Chief Delphi - FRC971 Spartan Robotics 2016 Release Video

Chief Delphi (http://www.chiefdelphi.com/forums/index.php)

- Robot Showcase (http://www.chiefdelphi.com/forums/forumdisplay.php?f=58)

- - FRC971 Spartan Robotics 2016 Release Video (http://www.chiefdelphi.com/forums/showthread.php?t=146113)

Re: FRC971 Spartan Robotics 2016 Release Video

Quote:

Originally Posted by ranlevinstein (Post 1593103)

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?

Code:

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

Re: FRC971 Spartan Robotics 2016 Release Video

Quote:

Originally Posted by AustinSchuh (Post 1592867)

The equation to minimize is:

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

This cost function is just the state error part of LQR, correct?

Quote:

Originally Posted by AustinSchuh (Post 1593165)

Code:

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

I noticed that this solution ends up evaluating to the (pseudo)inverse of B when Q is a constant multiple of the identity matrix, which is the solution to R(n+1)=A*R(n)+B*u when u=Kff*(R(n+1)-A*R(n)). What is the reasoning behind using the LQR weighted solution instead of the simpler version?

Re: FRC971 Spartan Robotics 2016 Release Video

You set your wheel velocity to 640 in your shooter code. I can't figure out what unit of measure this 640 is calculated in; RPM would be too slow while RPS would be too fast. What unit do you use, and is it related to your model based calculation of everything? (Do you have any tips for starting to learn how to run everything using models?)

Re: FRC971 Spartan Robotics 2016 Release Video

Quote:

Originally Posted by thatprogrammer (Post 1593240)

Radians per second, I would assume. Everything in standard units :D. Using radians makes the derivation of the models simpler, especially when the rotation eventually gets transformed into linear motion, and it is nice to be consistent across the board.

Re: FRC971 Spartan Robotics 2016 Release Video

Quote:

Originally Posted by ranlevinstein (Post 1593043)

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.

Quote:

Originally Posted by ranlevinstein (Post 1593043)

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.

Quote:

Originally Posted by ranlevinstein (Post 1593043)

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

Quote:

Originally Posted by ranlevinstein (Post 1593043)

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.

Re: FRC971 Spartan Robotics 2016 Release Video

Quote:

Originally Posted by kylestach1678 (Post 1593174)

This cost function is just the state error part of LQR, correct?

Yes. Since it doesn't have the U part of the LQR controller, it isn't provably stable, and I've seen ones which aren't...

Quote:

Originally Posted by kylestach1678 (Post 1593174)

Good catch. It worked, so we stopped? ;) I'd like to revisit that math this summer. It doesn't always return an answer which converges, which suggests to me that something is wrong with it.