FRC971 Spartan Robotics 2016 Release Video

[AdamHeard]

Amazing as always.

I count 5 control loops minimum to put a ball in the goal?

[AustinSchuh]

Originally Posted by AdamHeard

I count 5 control loops minimum to put a ball in the goal?

Depends on how you count them

The same loop runs on the left and right side of the shooter. They are only coupled in software. The control loop to run the shoulder and shooter angle is a 6 state MIMO (multiple input, multiple output) controller. The intake loop is separate. Then, there is the MIMO drivetrain loop being fed with angles by the camera.

So, yea, 5.

Originally Posted by aphelps231

May I ask what kind of mechanism you're using to push the ball into the shooter wheels?

A pair of connected linkages (driven by miniature pistons).

[Basel A]

Originally Posted by AustinSchuh

Depends on how you count them

The same loop runs on the left and right side of the shooter. They are only coupled in software. The control loop to run the shoulder and shooter angle is a 6 state MIMO (multiple input, multiple output) controller. The intake loop is separate. Then, there is the MIMO drivetrain loop being fed with angles by the camera.

So, yea, 5.



A pair of connected linkages (driven by miniature pistons).

The first 4 states are obvious: main arm position and speed, shooter mini-arm position and speed. What are the other two? How'd you go about tuning that? With that level of complexity, I imagine you had to go to model-based control?

[josesantos]

Originally Posted by Basel A

The first 4 states are obvious: main arm position and speed, shooter mini-arm position and speed. What are the other two? How'd you go about tuning that? With that level of complexity, I imagine you had to go to model-based control?

I'd guess they're also controlling acceleration for both arms. In 2014, they described their control system hardware and software in this thread.

971's made a really cool robot as usual, I'm hoping to see it in person soon!

[cxcad]

How much of this robot was made on your in house CNC router? Great looking robot by the way

[Basel A]

Originally Posted by josesantos

I'd guess they're also controlling acceleration for both arms. In 2014, they described their control system hardware and software in this thread.

I remember reading that thread now, totally forgot it existed. Thanks for the reminder. Would be fun to do this stuff on an FRC robot if we ever found the time..

[AustinSchuh]

Originally Posted by Basel A

The first 4 states are obvious: main arm position and speed, shooter mini-arm position and speed. What are the other two? How'd you go about tuning that? With that level of complexity, I imagine you had to go to model-based control?

Model based control is required Once you get the hang of it, I find it to let us do cooler stuff than non-model based controls. We plot things and try to figure out which terms have errors in them to help debug it.

The states are:
[shoulder position; shoulder velocity; shooter position (relative to the base), shooter velocity (relative to the base), shoulder voltage error, shooter voltage error]

The shooter is connected to the superstructure, but there is a coordinate transformation to have the states be relative to the ground. This gives us better control over what we actually care about.

The voltage errors are what we use instead of integral control. This lets the kalman filter learn the difference between what the motor is being asked to do and what actually is achieved, and lets us compensate for it. If you work the math out volts -> force.

[ranlevinstein]

Originally Posted by AustinSchuh

Model based control is required Once you get the hang of it, I find it to let us do cooler stuff than non-model based controls. We plot things and try to figure out which terms have errors in them to help debug it.

The states are:
[shoulder position; shoulder velocity; shooter position (relative to the base), shooter velocity (relative to the base), shoulder voltage error, shooter voltage error]

The shooter is connected to the superstructure, but there is a coordinate transformation to have the states be relative to the ground. This gives us better control over what we actually care about.

The voltage errors are what we use instead of integral control. This lets the kalman filter learn the difference between what the motor is being asked to do and what actually is achieved, and lets us compensate for it. If you work the math out volts -> force.

First of all your robot is truly amazing!

I have a few questions about your control.

1.I have read about your delta-u controller and I am not sure if I understood it correctly and I would like to know if i got it right. You have 3 states on your state space controller which include position , velocity and error voltage. you model it as (dx/dt) = Ax + Bu and u is the rate of change of voltage. Then you use pole placement to find the K matrix and then in your controller you set u to be -Kx. Then you estimate the state from position using an observer. you use an integrator for u and command the motors with it. To the error voltage state you feed in the difference between the estimated voltage from the observer and the integrated u commands.

2.Will the delta-u controller work the same if i will command the motors with u and not the integral of it and instead use the integral voltage error as state? why did you choose this way for the controller and not another form?

3.Is the delta-u controller in the end a linear combination of position error, velocity error and voltage error?

4.Why did you use Kalman filter instead of a regular observer? How much better was it in comparison to a regular observer?

5.How did you tune the Q and R matrices in the kalman filter?

6.How do you tune the parameters that transfrom the motion profile to the feed-forward you can feed to your motors?

7.How did you create 2 dimensional trajectories for your robot during auto?

8.How do you sync multiple trajectories in the auto period? for example how did you make the arm of your robot go up after crossing a defense?

Thank you very much!

[AustinSchuh]

Originally Posted by ranlevinstein

First of all your robot is truly amazing!

I have a few questions about your control.

1.I have read about your delta-u controller and I am not sure if I understood it correctly and I would like to know if i got it right. You have 3 states on your state space controller which include position , velocity and error voltage. you model it as (dx/dt) = Ax + Bu and u is the rate of change of voltage. Then you use pole placement to find the K matrix and then in your controller you set u to be -Kx. Then you estimate the state from position using an observer. you use an integrator for u and command the motors with it. To the error voltage state you feed in the difference between the estimated voltage from the observer and the integrated u commands.

2.Will the delta-u controller work the same if i will command the motors with u and not the integral of it and instead use the integral voltage error as state? why did you choose this way for the controller and not another form?

You nailed it.

Delta-U won't work if you command the motors with U, since your model doesn't match your plant (off by an integral).

I've recently switched formulations to what we used this and last year, and I think the new formulation is easier to understand.

If you have an un-augmented plant dx/dt = Ax + Bu, you can augment it by adding a "voltage error state".

d[x; voltage_error]/dt = [A, B; 0, 0] x + [B; 0] u

You then design the controller to control the original Ax + Bu (u =K * (R - X), design an observer to observe the augmented X, and then use a controller which is really [K, 1].

We switched over last year because it was easier to think about the new controller. In the end, both it and Delta U controllers will do the same thing.

Originally Posted by ranlevinstein

3.Is the delta-u controller in the end a linear combination of position error, velocity error and voltage error?

Yes. It's just another way to add integral into the mix. I like it because if your model is performing correctly, you won't get any integral windup. The trick is that it lets the applied voltage diverge from the voltage that the robot appears to be moving with by observing it in the observer.

Originally Posted by ranlevinstein

4.Why did you use Kalman filter instead of a regular observer? How much better was it in comparison to a regular observer?

It's just another way to tune a state space observer. If you check the math, if you assume fixed gains, the kalman gain converges to a fixed number as time evolves. You can solve for that kalman gain and use it all the time. Which results in the update step you find in an observer.

Honestly, I end up tuning it one way and then looking at the poles directly at the end to see how the tuning affected the results.

Originally Posted by ranlevinstein

5.How did you tune the Q and R matrices in the kalman filter?

The rule of thumb I've been using is to set the diagonal terms to the square of a reasonable error quantity for that term (for Q), and try to guess how much model uncertainty there is. I also like to look at the resulting kalman gain to see how crazy it is, and then also plot the input vs the output of the filter and look at how well it performs during robot moves. I've found that if I look at things from enough angles, I get a better picture of what's going on.

Originally Posted by ranlevinstein

6.How do you tune the parameters that transfrom the motion profile to the feed-forward you can feed to your motors?

I didn't. I defined a cost function to minimize the error between the trajectory every cycle and a feed-forward based goal (this made the goal feasabile), and used that to define a Kff.

The equation to minimize is:

(B * U - (R(n+1) - A R(n)))^T * Q * (B * U - (R(n+1) - A R(n)))

This means that you have 3 goals running around. The un-profiled goal, the profiled goal and the R that the feed-forwards is asking you to go to. I'd recommend you read the code to see how we kept track of it all, and I'm happy to answer questions from there.

The end result was that our model defined the feed-forwards constants, so it was free We also were able to gain schedule the feed-forwards terms for free as well.

FYI, this was the first year that we did feed-forwards. Before, we just relied on the controllers compensating. You can see it in some of the moves in the 2015 robot where it'll try to do a horizontal move, but end up with a steady state offset while moving due to the lack of feed-forwards.

Originally Posted by ranlevinstein

7.How did you create 2 dimensional trajectories for your robot during auto?

We cheated. We had a rotational trapezoidal motion profile and a linear trapezoidal motion profile. We just started them at different times/positions, added them to each other, and let them overlay on top of each-other. It was a pain to tune, but worked well enough. We are going to try to implement http://arl.cs.utah.edu/pubs/ACC2014.pdf this summer.

Originally Posted by ranlevinstein

8.How do you sync multiple trajectories in the auto period? for example how did you make the arm of your robot go up after crossing a defense?

Thank you very much!

Our auto code was a lot of "kick off A, wait until condition, kick off B, wait until condition, kick off C, ..." So, we'd start a motion profile in the drive, wait until we had moved X far, and then start the motion profile for the arm. The controllers would calculate the profiles as they went, so all Auto actually did was coordinate when to ask what subsystem to go where. With enough motion profiles, and when you make sure they aren't saturated, you end up with a pretty deterministic result.

Awesome questions, keep them coming! I love this stuff.

[ranlevinstein]

Originally Posted by AustinSchuh

I've recently switched formulations to what we used this and last year, and I think the new formulation is easier to understand.

If you have an un-augmented plant dx/dt = Ax + Bu, you can augment it by adding a "voltage error state".

d[x; voltage_error]/dt = [A, B; 0, 0] x + [B; 0] u

You then design the controller to control the original Ax + Bu (u =K * (R - X), design an observer to observe the augmented X, and then use a controller which is really [K, 1].

We switched over last year because it was easier to think about the new controller. In the end, both it and Delta U controllers will do the same thing.

Thank you for your fast reply!

Are the A and B matrices here the same as in this pdf?
https://www.chiefdelphi.com/forums/a...1&d=1419983380

Originally Posted by AustinSchuh

Yes. It's just another way to add integral into the mix. I like it because if your model is performing correctly, you won't get any integral windup. The trick is that it lets the applied voltage diverge from the voltage that the robot appears to be moving with by observing it in the observer.

I am a bit confused here. I integrated both sides of the equation and got:
u = constant * integral of position error + constant * integral of velocity error + constant * integral of voltage error

Isn't that a PI control + integral control of the voltage? This controller as far as I know should have integral windup. What am I missing?

Originally Posted by AustinSchuh

I defined a cost function to minimize the error between the trajectory every cycle and a feed-forward based goal (this made the goal feasabile), and used that to define a Kff.

The equation to minimize is:

(B * U - (R(n+1) - A R(n)))^T * Q * (B * U - (R(n+1) - A R(n)))

This means that you have 3 goals running around. The un-profiled goal, the profiled goal and the R that the feed-forwards is asking you to go to. I'd recommend you read the code to see how we kept track of it all, and I'm happy to answer questions from there.

The end result was that our model defined the feed-forwards constants, so it was free We also were able to gain schedule the feed-forwards terms for free as well.

WOW!
This is really smart!
I want to make sure I got it, Q is weight matrix and you are looking for the u vector to minimize the expression? In which way are you minimizing it? My current Idea is to set the derivative of the expression to zero and solve for u. Is that correct?
Did you get to this expression by claiming that R(n+1) = AR(n) + Bu where u is the correct feed forward?

Can you explain how did you observe the voltage?

Originally Posted by AustinSchuh

We are going to try to implement http://arl.cs.utah.edu/pubs/ACC2014.pdf this summer.

This looks very interesting, why did you choose this way instead of all the other available methods?
Also how does your students understand this paper? There are lot of things that needs to be known in order the understand it.
Who teaches your students all this stuff?
My team don't have any control's mentor and we are not sure if to move to model based control or not. Our main problem with it is that there are a lot of things that need to be taught and it's very hard to maintain the knowledge if we don't have any mentor that knows this. Do you have any advice?

Thank you very much!

[AustinSchuh]

Originally Posted by ranlevinstein

Thank you for your fast reply!

Are the A and B matrices here the same as in this pdf?
https://www.chiefdelphi.com/forums/a...1&d=1419983380

For this subsystem, yes. More generally, they may diverge, but that's a very good place to start.

Originally Posted by ranlevinstein

I am a bit confused here. I integrated both sides of the equation and got:
u = constant * integral of position error + constant * integral of velocity error + constant * integral of voltage error

Isn't that a PI control + integral control of the voltage? This controller as far as I know should have integral windup. What am I missing?

It's a little bit more tricky to reason about the controller that way than you think. The voltage error term is really the error between what you are telling the controller it should be commanding, and what it thinks it is commanding. If you feed in a 0 (the steady state value when the system should be stopped, this should change if you have a profile), it will be the difference between the estimated plant u, and 0. This will try to drive the estimated plant u to 0 by commanding voltage. u will also have position and derivative terms. Those terms will decay back to 0 some amount every cycle due to the third term. This lets them act more like traditional PD terms, since they can't integrate forever.

The trick is that the integrator is inside the observer, not the controller. The controller may be commanding 0 volts, but if the observer is observing motion where it shouldn't be, it will estimate that voltage is being applied. This means that the third term will start to integrate the commanded voltage to compensate. If the observer is observing the correct applied voltage, it won't do that.

You can shot this in simulation a lot easier than you can reason about it. That's one of the reasons I switched to the new controller formulation with a direct voltage error estimate. I could think about it easier.

Originally Posted by ranlevinstein

WOW!
This is really smart!
I want to make sure I got it, Q is weight matrix and you are looking for the u vector to minimize the expression? In which way are you minimizing it? My current Idea is to set the derivative of the expression to zero and solve for u. Is that correct?
Did you get to this expression by claiming that R(n+1) = AR(n) + Bu where u is the correct feed forward?

Bingo. We didn't get the equation done perfectly, so sometimes Kff isn't perfect. It helps to simulate it to make sure it performs perfectly before trying it on a bot.

That is the correct equation, nice! You then want to drive R to be at the profile as fast as possible.

Originally Posted by ranlevinstein

Can you explain how did you observe the voltage?

You can mathematically prove that the observer can observe the voltage as long as you tune it correctly. This is called observability, and can be calculated from some matrix products given A and C. For most controls people, that is enough of an explanation

Intuitively, you can think of the observer estimating where the next sensor reading should be, measuring what it got, and then attributing the error to some amount of error in each state. So, if the position is always reading higher than expected, it will slowly squeeze the error into the voltage error term, where it will finally influence the model to not always read high anymore. You'll then have a pretty good estimate of the voltage required to do what you are currently doing.

Originally Posted by ranlevinstein

This looks very interesting, why did you choose this way instead of all the other available methods?
Also how does your students understand this paper? There are lot of things that needs to be known in order the understand it.
Who teaches your students all this stuff?
My team don't have any control's mentor and we are not sure if to move to model based control or not. Our main problem with it is that there are a lot of things that need to be taught and it's very hard to maintain the knowledge if we don't have any mentor that knows this. Do you have any advice?

Thank you very much!

The tricky part of the math is that robots can't move sideways. This type of system is known as a non-holonomic system. It is an open research topic to do good non-holonomic control since it is nonlinear. This paper was recommended to me by Jared Russell, and the results in the results section is actually really good. This generates provably stable paths that are feasible. A-Star and all the graph based path planning algorithms struggle to generate feasible paths.

We have a small number of students who get really involved in the controls on 971. Some years are better than others, but that's how this type of thing goes. There is a lot of software on a robot that isn't controls. I'm going to see if the paper actually gets good results, and then work to involve students to see if we can fix some of the shortcomings with the model that one of the examples in the paper uses and help make improvements. I think that'll let me simplify the concept somewhat for them and get them playing around with the algorithm. I've yet to try to involve students in something this mathematically challenging, so I'll know more once I've pulled it off... I mostly mention the paper as something fun that you can do with controls in this context.

When I get the proper amount of interest and commitment, I sit down with the student and spend a significant amount of time teaching them how and why state space controllers work. I like to do it by rederiving some of the math to help demystify it, and having them work through examples. I've had students take that knowledge pretty far and do some pretty cool things with it. Teaching someone something this tricky is a lot of work. We tend to have about 1 student every year actually take the time to succeed. Sometimes more, sometimes less.

Doing model based controls without good help can be tricky. I honestly recommend most of the time to focus on writing test cases with simulations with more simple controllers (PID, for example) before you then start looking at model based controls. This gets you ready for what you need to do for more complicated controllers, and if you were to stop there having learned dependency injection and testing, that would already be an enormous success. The issue is that most of this stuff is upper division college level material, and is sometimes graduate level material. Take a subsystem on your robot, and try to write a model based controller for it over the off-season.

[ranlevinstein]

Originally Posted by AustinSchuh

If you have an un-augmented plant dx/dt = Ax + Bu, you can augment it by adding a "voltage error state".

d[x; voltage_error]/dt = [A, B; 0, 0] x + [B; 0] u

You then design the controller to control the original Ax + Bu (u =K * (R - X), design an observer to observe the augmented X, and then use a controller which is really [K, 1].

We switched over last year because it was easier to think about the new controller. In the end, both it and Delta U controllers will do the same thing.

I modeled it as you said and I got that:
acceleration = a * velocity + b * (voltage error) + b * u, where a and b are constants.
I am a bit confused about why this is true because the voltage error is in volts and u is volts/seconds so you are adding numbers with different units.

Originally Posted by AustinSchuh

It's a little bit more tricky to reason about the controller that way than you think. The voltage error term is really the error between what you are telling the controller it should be commanding, and what it thinks it is commanding. If you feed in a 0 (the steady state value when the system should be stopped, this should change if you have a profile), it will be the difference between the estimated plant u, and 0. This will try to drive the estimated plant u to 0 by commanding voltage. u will also have position and derivative terms. Those terms will decay back to 0 some amount every cycle due to the third term. This lets them act more like traditional PD terms, since they can't integrate forever.

The trick is that the integrator is inside the observer, not the controller. The controller may be commanding 0 volts, but if the observer is observing motion where it shouldn't be, it will estimate that voltage is being applied. This means that the third term will start to integrate the commanded voltage to compensate. If the observer is observing the correct applied voltage, it won't do that.

You can shot this in simulation a lot easier than you can reason about it. That's one of the reasons I switched to the new controller formulation with a direct voltage error estimate. I could think about it easier.

I am still having some problems with understanding it, if the system is behaving just like it should then the integral of the voltage error will be zero and then there is just a PI controller. In my mind it makes a lot more sense to have:
u = constant * position error + constant * velocity error + constant * integral of voltage error
Maybe there is a problem with velocity error part here but I still don't understand how there won't be an integral windup when you have integral of position error in your controller.
What am I missing?

Also I saw you are using moment of inertia of what being spun in your model, What units is it and how can I find it?

Originally Posted by AustinSchuh

Bingo. We didn't get the equation done perfectly, so sometimes Kff isn't perfect. It helps to simulate it to make sure it performs perfectly before trying it on a bot.

That is the correct equation, nice! You then want to drive R to be at the profile as fast as possible.

I am having some problems with taking the derivative of the expression when I am leaving all the matrices as parameters. How did you do it? Did you get a parametric solution?

I was wondering about how the delta-u controller works when the u command get's higher than 12 volts, because then you can't control the rate of change of the voltage anymore.

Thank you so much! Your answers helped my team and me a lot!

[AustinSchuh]

Originally Posted by ranlevinstein

I modeled it as you said and I got that:
acceleration = a * velocity + b * (voltage error) + b * u, where a and b are constants.
I am a bit confused about why this is true because the voltage error is in volts and u is volts/seconds so you are adding numbers with different units.

Nice catch. Maybe I wasn't clear, but U changed units from volts/sec to volts, and the integrator on the output of the plant disappeared.

Originally Posted by ranlevinstein

I am still having some problems with understanding it, if the system is behaving just like it should then the integral of the voltage error will be zero and then there is just a PI controller. In my mind it makes a lot more sense to have:
u = constant * position error + constant * velocity error + constant * integral of voltage error
Maybe there is a problem with velocity error part here but I still don't understand how there won't be an integral windup when you have integral of position error in your controller.
What am I missing?

I *think* you are off by an integrator again.

u = Kp * x + Kv * v + voltage_error

So, if torque_error = 0 (the model is behaving as expected), then you don't add anything. Ask again if I read too fast.

Originally Posted by ranlevinstein

Also I saw you are using moment of inertia of what being spun in your model, What units is it and how can I find it?

kg * m^2

Originally Posted by ranlevinstein

I was wondering about how the delta-u controller works when the u command get's higher than 12 volts, because then you can't control the rate of change of the voltage anymore.

Thank you so much! Your answers helped my team and me a lot!

You have the same problem with a normal controller. The nonlinear assumption breaks down there too. We just cap the accumulator to +- 12.

To solve that all correctly, you'll want to use a Model Predictive Controller. They are able to actually take into account saturation correctly. Unfortunately, they aren't easy to work with. We haven't deployed one yet to our robot. (Go read up on them a bit. They are super cool That was either one of, or my favorite class at college.)

It's been another busy week. Sorry for taking so long. I started a reply a week ago and couldn't find time to finish it.

[ranlevinstein]

Originally Posted by AustinSchuh

I defined a cost function to minimize the error between the trajectory every cycle and a feed-forward based goal (this made the goal feasabile), and used that to define a Kff.

The equation to minimize is:

(B * U - (R(n+1) - A R(n)))^T * Q * (B * U - (R(n+1) - A R(n)))

I managed to solve for u assuming Q is symmetric and the trajectory is feasible. I got:
u = (B^T *Q*B)^-1 * (r(n+1)^T - r(n)^T * A^T)*Q*B
Is that correct?

[AustinSchuh]

Originally Posted by ranlevinstein

I managed to solve for u assuming Q is symmetric and the trajectory is feasible. I got:
u = (B^T *Q*B)^-1 * (r(n+1)^T - r(n)^T * A^T)*Q*B
Is that correct?

def TwoStateFeedForwards(B, Q):
  """Computes the feed forwards constant for a 2 state controller.

  This will take the form U = Kff * (R(n + 1) - A * R(n)), where Kff is the
  feed-forwards constant.  It is important that Kff is *only* computed off
  the goal and not the feed back terms.

  Args:
    B: numpy.Matrix[num_states, num_inputs] The B matrix.
    Q: numpy.Matrix[num_states, num_states] The Q (cost) matrix.

  Returns:
    numpy.Matrix[num_inputs, num_states]
  """

  # We want to find the optimal U such that we minimize the tracking cost.
  # This means that we want to minimize
  #   (B * U - (R(n+1) - A R(n)))^T * Q * (B * U - (R(n+1) - A R(n)))
  # TODO(austin): This doesn't take into account the cost of U

  return numpy.linalg.inv(B.T * Q * B) * B.T * Q.T