예제 #1
0
    def test_simulation(self):
        # Basic constant-torque acrobot simulation.
        acrobot = AcrobotPlant()

        # Create the simulator.
        simulator = Simulator(acrobot)
        context = simulator.get_mutable_context()

        # Set an input torque.
        input = AcrobotInput()
        input.set_tau(1.)
        acrobot.GetInputPort("elbow_torque").FixValue(context, input)

        # Set the initial state.
        state = context.get_mutable_continuous_state_vector()
        state.set_theta1(1.)
        state.set_theta1dot(0.)
        state.set_theta2(0.)
        state.set_theta2dot(0.)

        self.assertTrue(acrobot.DynamicsBiasTerm(context).shape == (2, ))
        self.assertTrue(acrobot.MassMatrix(context).shape == (2, 2))
        initial_total_energy = acrobot.EvalPotentialEnergy(context) + \
            acrobot.EvalKineticEnergy(context)

        # Simulate (and make sure the state actually changes).
        initial_state = state.CopyToVector()
        simulator.AdvanceTo(1.0)

        self.assertLessEqual(
            acrobot.EvalPotentialEnergy(context) +
            acrobot.EvalKineticEnergy(context), initial_total_energy)
예제 #2
0
class EnergyShapingControl(LeafSystem):
    def __init__(self, k1, k2, k3, lqr_angle_range=1.0):
        super().__init__()
        self.k1 = k1
        self.k2 = k2
        self.k3 = k3
        self.acrobot_plant = AcrobotPlant()
        self.acrobot_context = self.acrobot_plant.CreateDefaultContext()
        self.upright_context = self.acrobot_plant.CreateDefaultContext()
        self.upright_context.get_mutable_continuous_state()\
            .SetFromVector(np.array([np.pi, 0, 0, 0]))
        self.lqr_angle_range = lqr_angle_range

        self.DeclareInputPort('estimated state', PortDataType.kVectorValued, 4)
        self.DeclareVectorOutputPort('control', BasicVector(1),
                                     self.calculate_control)
        self.Q = np.diag((10., 10., 1., 1.))
        self.R = [10]
        self.K = self.calculate_lqr()
        self.LQR_on = False

    def calculate_lqr(self):
        upright_plant = AcrobotPlant()
        upright_plant_context = upright_plant.CreateDefaultContext()
        upright_plant_context.get_mutable_continuous_state() \
            .SetFromVector(np.array([np.pi, 0, 0, 0]))
        upright_plant.GetInputPort("elbow_torque") \
            .FixValue(upright_plant_context, 0)

        lqr = LinearQuadraticRegulator(upright_plant, upright_plant_context,
                                       self.Q, self.R)
        return lqr.D()

    def calculate_control(self, context, control_vec):
        input = self.get_input_port(0).Eval(context)
        theta1 = input[0]
        theta2 = input[1]
        theta1_dot = input[2]
        theta2_dot = input[3]

        # Current acrobot context based on input.
        self.acrobot_context \
            .get_mutable_continuous_state() \
            .SetFromVector(input)

        desired_state = np.array([np.pi, 0, 0, 0])
        cost = (desired_state - input).T @ self.Q @ (desired_state - input)
        should_use_lqr = cost < 1

        if not self.LQR_on:
            self.LQR_on = should_use_lqr
            if should_use_lqr:
                print("LQR on {0}".format(context.get_time()))

        if self.LQR_on:
            u = -self.K @ (desired_state - input)
            control_vec.SetFromVector(u)
        else:
            # Bias term of the dynamics.
            bias = self.acrobot_plant.DynamicsBiasTerm(self.acrobot_context)

            # Mass matrix
            M = self.acrobot_plant.MassMatrix(self.acrobot_context)
            m11 = M[0, 0]
            m12 = M[0, 1]
            m21 = M[1, 0]
            m22 = M[1, 1]
            m22_bar = m22 - m21 * m12 / m11
            bias_bar = bias[1] - m21 / m11 * bias[0]

            # Desired energy, upright configuration.
            Ed = self.acrobot_plant.EvalPotentialEnergy(self.upright_context) \
                 + self.acrobot_plant.EvalKineticEnergy(self.upright_context)
            # Current energy.
            Ec = self.acrobot_plant.EvalPotentialEnergy(self.acrobot_context) \
                 + self.acrobot_plant.EvalKineticEnergy(self.acrobot_context)
            E_error = Ec - Ed

            u_bar = E_error * theta1_dot

            u = -self.k1 * theta2 - self.k2 * theta2_dot + self.k3 * u_bar
            tau = m22_bar * u + bias_bar

            control_vec.SetFromVector(np.array([tau]))