3

Background

I am running direct colocation and feeding the result into FiniteHorizonLinearQuadraticRegulator to stabilize the trajectory, but I havee run into an error when constructing the FHLQR.

I am trying to understand what the following means:

---------------------------------------------------------------------------
RuntimeError                              Traceback (most recent call last)
<ipython-input-40-0a8e3d5cdef4> in <module>
     10 Q = np.diag([10.,10.,1.,1.])
     11 options.Qf = Q
---> 12 tvlqr = MakeFiniteHorizonLinearQuadraticRegulator(
     13                                       plant, context, t0=options.u0.start_time(),
     14                                       tf=options.u0.end_time(), Q=Q, R=np.eye(1),

RuntimeError: The following environment does not have an entry for the variable u(0)
t -> 4.46101
z2 -> 3.18212e-06
z3 -> 0.001
z1 -> 3
z4 -> 2.82268e-05
u1 -> 0

What I've tried

At first I thought I might need to name my input variable to exactly u but that did not help. I am thinking that u(0) means it cannot find a place to put the input at the first time step but I am not sure how I can fix this.

I tried to fix the input port for the plant at time zero to a value of 0 using this: plant.get_input_port(0).FixValue(context, [0]) but it still results in the same problem.

I also tried setting the input_port_index option like so: options.input_port_index = plant.get_input_port(0).get_index() but I get the same error.

I think this is not a problem with how I set up the FiniteHorizonLinearQuadraticRegulator because I replaced MakeFiniteHorizonLinearQuadraticRegulator with a LinearQuadraticRegulator and have the same error.

I would really appreciate some insight into what this error means and how to resolve it.

Code Skeleton

t = Variable("t") # time

# state of the robot (in cartesian coordinates)
z1 = Variable("z1") # ball angle (phi)
z2 = Variable("z2") # body angle 
z3 = Variable("z3") # ball angular velocity (nu)
z4 = Variable("z4") # body angular velocity
cartesian_state = [z1, z2, z3, z4]

# control input of the robot
u1 = Variable("u1") # wheel torque
ctrl_input = [u1]

# nonlinear dynamics, the whole state is measured (output = state)
## dynamics equations here ##
qddot = ((B*u-C-G)/M).T[0]

dynamics = np.concatenate(([z3, z4], qddot))

# dynamics = [u1*cos(z3), u1*sin(z3), u2]
robot = SymbolicVectorSystem(
    time=t,
    state=cartesian_state,
    input=ctrl_input,
    output=cartesian_state,
    dynamics=dynamics,
)

options = FiniteHorizonLinearQuadraticRegulatorOptions()
options.x0 = dircol.ReconstructStateTrajectory(result)
options.u0 = dircol.ReconstructInputTrajectory(result)

builder = DiagramBuilder()
plant = builder.AddSystem(robot)
context = plant.CreateDefaultContext()
Q = np.diag([10.,10.,1.,1.])
options.Qf = Q
tvlqr = MakeFiniteHorizonLinearQuadraticRegulator(
                                      plant, context, t0=options.u0.start_time(), 
                                      tf=options.u0.end_time(), Q=Q, R=np.eye(1),
                                      options=options)
regulator = builder.AddSystem(tvlqr)
Eric Boehlke
  • 31
  • 2

1 Answers1

0

I realized that I was using u in my qddot calculation when I meant to be using u1. Further up in my notebook I defined u = dircol.input() which is where the error was coming from.

Eric Boehlke
  • 31
  • 2