def test_diagram_adder(self):
     system = CustomDiagram(2, 3)
     self.assertEqual(system.num_input_ports(), 2)
     with catch_drake_warnings(expected_count=1):
         system.get_num_input_ports()
     self.assertEqual(system.get_input_port(0).size(), 3)
     self.assertEqual(system.num_output_ports(), 1)
     with catch_drake_warnings(expected_count=1):
         system.get_num_output_ports()
     self.assertEqual(system.get_output_port(0).size(), 3)

    def test_simulator_ctor(self):
        # Tests a simple simulation for supported scalar types.
        for T in [float, AutoDiffXd]:
            # Create simple system.
            system = ConstantVectorSource_[T]([1.])

            def check_output(context):
                # Check number of output ports and value for a given context.
                output = system.AllocateOutput()
                self.assertEqual(output.num_ports(), 1)
                system.CalcOutput(context, output)
                if T == float:
                    value = output.get_vector_data(0).get_value()
                    self.assertTrue(np.allclose([1], value))
                elif T == AutoDiffXd:
                    value = output.get_vector_data(0)._get_value_copy()
                    # TODO(eric.cousineau): Define `isfinite` ufunc, if
                    # possible, to use for `np.allclose`.
                    self.assertEqual(value.shape, (1,))
                    self.assertEqual(value[0], AutoDiffXd(1.))
                else:
                    raise RuntimeError("Bad T: {}".format(T))

            # Create simulator with basic constructor.
            simulator = Simulator_[T](system)
            simulator.Initialize()
            simulator.set_target_realtime_rate(0)
            simulator.set_publish_every_time_step(True)
            self.assertTrue(simulator.get_context() is
                            simulator.get_mutable_context())
            check_output(simulator.get_context())
            simulator.AdvanceTo(1)

            # Create simulator specifying context.
            context = system.CreateDefaultContext()
            with catch_drake_warnings(expected_count=1):
                context.set_time(0.)
            context.SetTime(0.)

            with catch_drake_warnings(expected_count=1):
                context.set_accuracy(1e-4)
            context.SetAccuracy(1e-4)
            self.assertEqual(context.get_accuracy(), 1e-4)

            # @note `simulator` now owns `context`.
            simulator = Simulator_[T](system, context)
            self.assertTrue(simulator.get_context() is context)
            check_output(context)
            simulator.AdvanceTo(1)

    def test_AddPositionConstraint(self):
        p_BQ = np.array([0.2, 0.3, 0.5])
        p_AQ_lower = np.array([-0.1, -0.2, -0.3])
        p_AQ_upper = np.array([-0.05, -0.12, -0.28])

        self.ik_two_bodies.AddPositionConstraint(
            frameB=self.body1_frame, p_BQ=p_BQ,
            frameA=self.body2_frame,
            p_AQ_lower=p_AQ_lower, p_AQ_upper=p_AQ_upper)
        result = mp.Solve(self.prog)
        self.assertTrue(result.is_success())
        q_val = result.GetSolution(self.q)

        body1_quat = self._body1_quat(q_val)
        body1_pos = self._body1_xyz(q_val)
        body2_quat = self._body2_quat(q_val)
        body2_pos = self._body2_xyz(q_val)
        body1_rotmat = Quaternion(body1_quat).rotation()
        body2_rotmat = Quaternion(body2_quat).rotation()
        p_AQ = body2_rotmat.transpose().dot(
            body1_rotmat.dot(p_BQ) + body1_pos - body2_pos)
        self.assertTrue(np.greater(p_AQ, p_AQ_lower -
                                   1E-6 * np.ones((3, 1))).all())
        self.assertTrue(np.less(p_AQ, p_AQ_upper +
                                1E-6 * np.ones((3, 1))).all())

        with catch_drake_warnings(expected_count=2):
            self.assertEqual(
                self.prog.Solve(), mp.SolutionResult.kSolutionFound)
            self.assertTrue(np.allclose(
                self.prog.GetSolution(self.q), q_val))

 def test_mock_lcm_induce_subscriber_callback_deprecated(self):
     dut = DrakeMockLcm()
     dut.Subscribe(channel="TEST_CHANNEL", handler=self._handler)
     with catch_drake_warnings(expected_count=1):
         dut.InduceSubscriberCallback(
             channel="TEST_CHANNEL", buffer=self.quat.encode())
     self.assertEqual(self.count, 1)

    def test_AddAngleBetweenVectorsConstraint(self):
        na_A = np.array([0.2, -0.4, 0.9])
        nb_B = np.array([1.4, -0.1, 1.8])

        angle_lower = 0.2 * math.pi
        angle_upper = 0.2 * math.pi

        self.ik_two_bodies.AddAngleBetweenVectorsConstraint(
            frameA=self.body1_frame, na_A=na_A,
            frameB=self.body2_frame, nb_B=nb_B,
            angle_lower=angle_lower, angle_upper=angle_upper)
        result = mp.Solve(self.prog)
        self.assertTrue(result.is_success())

        q_val = result.GetSolution(self.q)
        body1_quat = self._body1_quat(q_val)
        body2_quat = self._body2_quat(q_val)
        body1_rotmat = Quaternion(body1_quat).rotation()
        body2_rotmat = Quaternion(body2_quat).rotation()

        na_W = body1_rotmat.dot(na_A)
        nb_W = body2_rotmat.dot(nb_B)

        angle = math.acos(na_W.transpose().dot(nb_W) /
                          (np.linalg.norm(na_W) * np.linalg.norm(nb_W)))

        self.assertLess(math.fabs(angle - angle_lower), 1E-6)

        with catch_drake_warnings(expected_count=2):
            self.assertEqual(
                self.prog.Solve(), mp.SolutionResult.kSolutionFound)
            self.assertTrue(np.allclose(
                self.prog.GetSolution(self.q), q_val))

    def test_deprecated_protected_aliases(self):
        """Tests a subset of protected aliases, pursuant to #9651."""

        class OldSystem(LeafSystem):
            def __init__(self):
                LeafSystem.__init__(self)
                self.called_publish = False
                # Check a non-overridable method
                with catch_drake_warnings(expected_count=1):
                    self._DeclareVectorInputPort("x", BasicVector(1))

            def _DoPublish(self, context, events):
                self.called_publish = True

        # Ensure old overrides are still used
        system = OldSystem()
        context = system.CreateDefaultContext()
        with catch_drake_warnings(expected_count=1):
            system.Publish(context)
        self.assertTrue(system.called_publish)

        # Ensure documentation doesn't duplicate stuff.
        with catch_drake_warnings(expected_count=1):
            self.assertIn("deprecated", LeafSystem._DoPublish.__doc__)
        # This will warn both on (a) calling the method and (b) on the
        # invocation of the override.
        with catch_drake_warnings(expected_count=2):
            LeafSystem._DoPublish(system, context, [])

        class AccidentallyBothSystem(LeafSystem):
            def __init__(self):
                LeafSystem.__init__(self)
                self.called_old_publish = False
                self.called_new_publish = False

            def DoPublish(self, context, events):
                self.called_new_publish = True

            def _DoPublish(self, context, events):
                self.called_old_publish = True

        system = AccidentallyBothSystem()
        context = system.CreateDefaultContext()
        # This will trigger no deprecations, as the newer publish is called.
        system.Publish(context)
        self.assertTrue(system.called_new_publish)
        self.assertFalse(system.called_old_publish)

    def test_deprecated_parsing(self):
        sdf_file = FindResourceOrThrow(
            "drake/multibody/benchmarks/acrobot/acrobot.sdf")

        plant = MultibodyPlant(time_step=0.01)
        with catch_drake_warnings(expected_count=1):
            result = AddModelFromSdfFile(plant=plant, file_name=sdf_file)
            self.assertIsInstance(result, ModelInstanceIndex)

 def test_ipopt_solver_deprecated(self):
     # This serves as the (only) unit test coverage for the deprecated
     # SolverBase method that writes the solution back into the program.
     prog, x, x_expected = self._make_prog()
     solver = IpoptSolver()
     with catch_drake_warnings(expected_count=2):
         result = solver.Solve(prog)
         solution = prog.GetSolution(x)
     self.assertEqual(result, mp.SolutionResult.kSolutionFound)
     self.assertTrue(np.allclose(solution, x_expected))

 def test_lcm(self):
     dut = DrakeLcm()
     self.assertIsInstance(dut, DrakeLcmInterface)
     # Quickly start and stop the receiving thread.
     with catch_drake_warnings(expected_count=2):
         dut.StartReceiveThread()
         dut.StopReceiveThread()
     # Test virtual function names.
     dut.Publish
     dut.HandleSubscriptions

 def DoHasDirectFeedthrough(self, input_port, output_port):
     # Test inputs.
     test.assertIn(input_port, [0, 1])
     test.assertEqual(output_port, 0)
     # Call base method to ensure we do not get recursion.
     with catch_drake_warnings(expected_count=1):
         base_return = LeafSystem.DoHasDirectFeedthrough(
             self, input_port, output_port)
     test.assertTrue(base_return is None)
     # Return custom methods.
     self.called_feedthrough = True
     return False

    def test_deprecated_tree_api(self):
        plant = MultibodyPlant()
        plant.Finalize()

        with catch_drake_warnings() as w:
            num_expected_warnings = [0]

            def expect_new_warning(msg_part):
                num_expected_warnings[0] += 1
                self.assertEqual(len(w), num_expected_warnings[0])
                self.assertIn(msg_part, str(w[-1].message))

            tree = plant.tree()
            expect_new_warning("`tree()`")
            MobilizerIndex(0)
            expect_new_warning("`MobilizerIndex`")
            BodyNodeIndex(0)
            expect_new_warning("`BodyNodeIndex`")
            MultibodyForces(model=tree)
            expect_new_warning("`MultibodyForces(plant)`")
            element = plant.world_body()
            self.assertIsInstance(element.get_parent_tree(), MultibodyTree)
            expect_new_warning("`get_parent_tree()`")

        # Check old spellings (no deprecation warnings).
        self.check_old_spelling_exists(tree.CalcRelativeTransform)
        self.check_old_spelling_exists(tree.CalcPointsPositions)
        self.check_old_spelling_exists(
            tree.CalcFrameGeometricJacobianExpressedInWorld)
        self.check_old_spelling_exists(tree.EvalBodyPoseInWorld)
        self.check_old_spelling_exists(tree.SetFreeBodyPoseOrThrow)
        self.check_old_spelling_exists(tree.SetFreeBodySpatialVelocityOrThrow)
        self.check_old_spelling_exists(tree.EvalBodySpatialVelocityInWorld)
        self.check_old_spelling_exists(tree.GetPositionsFromArray)
        self.check_old_spelling_exists(tree.GetVelocitiesFromArray)
        self.check_old_spelling_exists(tree.CalcMassMatrixViaInverseDynamics)
        self.check_old_spelling_exists(tree.CalcBiasTerm)
        self.check_old_spelling_exists(tree.CalcInverseDynamics)
        self.check_old_spelling_exists(tree.num_frames)
        self.check_old_spelling_exists(tree.get_body)
        self.check_old_spelling_exists(tree.get_joint)
        self.check_old_spelling_exists(tree.get_joint_actuator)
        self.check_old_spelling_exists(tree.get_frame)
        self.check_old_spelling_exists(tree.GetModelInstanceName)

        context = plant.CreateDefaultContext()
        # All body poses.
        X_WB, = tree.CalcAllBodyPosesInWorld(context)
        self.assertIsInstance(X_WB, Isometry3)
        v_WB, = tree.CalcAllBodySpatialVelocitiesInWorld(context)
        self.assertIsInstance(v_WB, SpatialVelocity)

    def test_eval_binding(self):
        qp = TestQP()
        prog = qp.prog

        x = qp.x
        x_expected = np.array([1., 1.])

        costs = qp.costs
        cost_values_expected = [2., 1.]
        constraints = qp.constraints
        constraint_values_expected = [1., 1., 2., 3.]

        with catch_drake_warnings(action='ignore'):
            prog.Solve()
            self.assertTrue(np.allclose(prog.GetSolution(x), x_expected))

            enum = zip(constraints, constraint_values_expected)
            for (constraint, value_expected) in enum:
                value = prog.EvalBindingAtSolution(constraint)
                self.assertTrue(np.allclose(value, value_expected))

            enum = zip(costs, cost_values_expected)
            for (cost, value_expected) in enum:
                value = prog.EvalBindingAtSolution(cost)
                self.assertTrue(np.allclose(value, value_expected))

            # Existence check for non-autodiff versions.
            self.assertIsInstance(
                prog.EvalBinding(costs[0], x_expected), np.ndarray)
            self.assertIsInstance(
                prog.EvalBindings(prog.GetAllConstraints(), x_expected),
                np.ndarray)

            # Existence check for autodiff versions.
            self.assertIsInstance(
                jacobian(partial(prog.EvalBinding, costs[0]), x_expected),
                np.ndarray)
            self.assertIsInstance(
                jacobian(partial(prog.EvalBindings, prog.GetAllConstraints()),
                         x_expected),
                np.ndarray)

            # Bindings for `Eval`.
            x_list = (float(1.), AutoDiffXd(1.), sym.Variable("x"))
            T_y_list = (float, AutoDiffXd, sym.Expression)
            evaluator = costs[0].evaluator()
            for x_i, T_y_i in zip(x_list, T_y_list):
                y_i = evaluator.Eval(x=[x_i, x_i])
                self.assertIsInstance(y_i[0], T_y_i)

    def test_AddGazeTargetConstraint(self):
        p_AS = np.array([0.1, 0.2, 0.3])
        n_A = np.array([0.3, 0.5, 1.2])
        p_BT = np.array([1.1, 0.2, 1.5])
        cone_half_angle = 0.2 * math.pi

        self.ik_two_bodies.AddGazeTargetConstraint(
            frameA=self.body1_frame, p_AS=p_AS, n_A=n_A,
            frameB=self.body2_frame, p_BT=p_BT,
            cone_half_angle=cone_half_angle)

        result = mp.Solve(self.prog)
        self.assertTrue(result.is_success())
        q_val = result.GetSolution(self.q)

        body1_quat = self._body1_quat(q_val)
        body1_pos = self._body1_xyz(q_val)
        body2_quat = self._body2_quat(q_val)
        body2_pos = self._body2_xyz(q_val)
        body1_rotmat = Quaternion(body1_quat).rotation()
        body2_rotmat = Quaternion(body2_quat).rotation()

        p_WS = body1_pos + body1_rotmat.dot(p_AS)
        p_WT = body2_pos + body2_rotmat.dot(p_BT)
        p_ST_W = p_WT - p_WS
        n_W = body1_rotmat.dot(n_A)
        self.assertGreater(p_ST_W.dot(n_W), np.linalg.norm(
            p_ST_W) * np.linalg.norm(n_W) * math.cos(cone_half_angle) - 1E-6)

        with catch_drake_warnings(expected_count=2):
            self.assertEqual(
                self.prog.Solve(), mp.SolutionResult.kSolutionFound)
            q_val = self.prog.GetSolution(self.q)

            body1_quat = self._body1_quat(q_val)
            body1_pos = self._body1_xyz(q_val)
            body2_quat = self._body2_quat(q_val)
            body2_pos = self._body2_xyz(q_val)
            body1_rotmat = Quaternion(body1_quat).rotation()
            body2_rotmat = Quaternion(body2_quat).rotation()

            p_WS = body1_pos + body1_rotmat.dot(p_AS)
            p_WT = body2_pos + body2_rotmat.dot(p_BT)
            p_ST_W = p_WT - p_WS
            n_W = body1_rotmat.dot(n_A)
            self.assertGreater(p_ST_W.dot(n_W), np.linalg.norm(
                p_ST_W) * np.linalg.norm(n_W) *
                math.cos(cone_half_angle) - 1E-6)

    def test_sos(self):
        # Find a,b,c,d subject to
        # a(0) + a(1)*x,
        # b(0) + 2*b(1)*x + b(2)*x^2 is SOS,
        # c(0)*x^2 + 2*c(1)*x*y + c(2)*y^2 is SOS,
        # d(0)*x^2 is SOS.
        # d(1)*x^2 is SOS.
        prog = mp.MathematicalProgram()
        x = prog.NewIndeterminates(1, "x")
        poly = prog.NewFreePolynomial(sym.Variables(x), 1)
        (poly, binding) = prog.NewSosPolynomial(
            indeterminates=sym.Variables(x), degree=2)
        y = prog.NewIndeterminates(1, "y")
        (poly, binding) = prog.NewSosPolynomial(
            monomial_basis=(sym.Monomial(x[0]), sym.Monomial(y[0])))
        d = prog.NewContinuousVariables(2, "d")
        prog.AddSosConstraint(d[0]*x.dot(x))
        prog.AddSosConstraint(d[1]*x.dot(x), [sym.Monomial(x[0])])
        result = mp.Solve(prog)
        self.assertTrue(result.is_success())

        # Test SubstituteSolution(sym.Expression)
        with catch_drake_warnings(action='ignore'):
            prog.Solve()
            # TODO(eric.cousineau): Expose `SymbolicTestCase` so that other
            # tests can use the assertion utilities.
            self.assertEqual(
                prog.SubstituteSolution(d[0] + d[1]).Evaluate(),
                prog.GetSolution(d[0]) + prog.GetSolution(d[1]))
            # Test SubstituteSolution(sym.Polynomial)
            poly = d[0]*x.dot(x)
            poly_sub_actual = prog.SubstituteSolution(
                sym.Polynomial(poly, sym.Variables(x)))
            poly_sub_expected = sym.Polynomial(
                prog.SubstituteSolution(d[0])*x.dot(x), sym.Variables(x))
            # TODO(soonho): At present, these must be converted to `Expression`
            # to compare, because as `Polynomial`s the comparison fails with
            # `0*x(0)^2` != `0`, which indicates that simplification is not
            # happening somewhere.
            self.assertTrue(
                poly_sub_actual.ToExpression().EqualTo(
                    poly_sub_expected.ToExpression()),
                "{} != {}".format(poly_sub_actual, poly_sub_expected))

    def _do_test_direct_transcription(self, use_deprecated_solve):
        # Integrator.
        plant = LinearSystem(A=[0.], B=[1.], C=[1.], D=[0.], time_period=0.1)
        context = plant.CreateDefaultContext()

        dirtran = DirectTranscription(plant, context, num_time_samples=21)

        # Spell out most of the methods, regardless of whether they make sense
        # as a consistent optimization.  The goal is to check the bindings,
        # not the implementation.
        t = dirtran.time()
        dt = dirtran.fixed_timestep()
        x = dirtran.state()
        x2 = dirtran.state(2)
        x0 = dirtran.initial_state()
        xf = dirtran.final_state()
        u = dirtran.input()
        u2 = dirtran.input(2)

        dirtran.AddRunningCost(x.dot(x))
        dirtran.AddConstraintToAllKnotPoints(u[0] == 0)
        dirtran.AddFinalCost(2*x.dot(x))

        initial_u = PiecewisePolynomial.ZeroOrderHold([0, .3*21],
                                                      np.zeros((1, 2)))
        initial_x = PiecewisePolynomial()
        dirtran.SetInitialTrajectory(initial_u, initial_x)

        if use_deprecated_solve:
            with catch_drake_warnings(expected_count=6):
                dirtran.Solve()
                times = dirtran.GetSampleTimes()
                inputs = dirtran.GetInputSamples()
                states = dirtran.GetStateSamples()
                input_traj = dirtran.ReconstructInputTrajectory()
                state_traj = dirtran.ReconstructStateTrajectory()
        else:
            result = mp.Solve(dirtran)
            times = dirtran.GetSampleTimes(result)
            inputs = dirtran.GetInputSamples(result)
            states = dirtran.GetStateSamples(result)
            input_traj = dirtran.ReconstructInputTrajectory(result)
            state_traj = dirtran.ReconstructStateTrajectory(result)

    def test_abstract_io_port(self):
        test = self
        # N.B. Since this has trivial operations, we can test all scalar types.
        for T in [float, AutoDiffXd, Expression]:
            default_value = ("default", T(0.))
            expected_input_value = ("input", T(np.pi))
            expected_output_value = ("output", 2*T(np.pi))

            class CustomAbstractSystem(LeafSystem_[T]):
                def __init__(self):
                    LeafSystem_[T].__init__(self)
                    self.input_port = self.DeclareAbstractInputPort(
                        "in", AbstractValue.Make(default_value))
                    self.output_port = self.DeclareAbstractOutputPort(
                        "out",
                        lambda: AbstractValue.Make(default_value),
                        self.DoCalcAbstractOutput,
                        prerequisites_of_calc=set([
                            self.input_port.ticket()]))

                def DoCalcAbstractOutput(self, context, y_data):
                    input_value = self.EvalAbstractInput(
                        context, 0).get_value()
                    # The allocator function will populate the output with
                    # the "input"
                    test.assertTupleEqual(input_value, expected_input_value)
                    y_data.set_value(expected_output_value)
                    test.assertTupleEqual(y_data.get_value(),
                                          expected_output_value)

            system = CustomAbstractSystem()
            context = system.CreateDefaultContext()

            self.assertEqual(context.num_input_ports(), 1)
            context.FixInputPort(0, AbstractValue.Make(expected_input_value))
            output = system.AllocateOutput()
            self.assertEqual(output.num_ports(), 1)
            with catch_drake_warnings(expected_count=1):
                output.get_num_ports()
            system.CalcOutput(context, output)
            value = output.get_data(0)
            self.assertEqual(value.get_value(), expected_output_value)

    def test_AddMinimumDistanceConstraint(self):
        ik = self.ik_two_bodies
        W = self.plant.world_frame()
        B1 = self.body1_frame
        B2 = self.body2_frame

        min_distance = 0.1
        tol = 1e-2
        radius1 = 0.1
        radius2 = 0.2

        ik.AddMinimumDistanceConstraint(minimal_distance=min_distance)
        context = self.plant.CreateDefaultContext()
        R_I = np.eye(3)
        self.plant.SetFreeBodyPose(
            context, B1.body(), Isometry3(R_I, [0, 0, 0.01]))
        self.plant.SetFreeBodyPose(
            context, B2.body(), Isometry3(R_I, [0, 0, -0.01]))

        def get_min_distance_actual():
            X = partial(self.plant.CalcRelativeTransform, context)
            distance = norm(X(W, B1).translation() - X(W, B2).translation())
            return distance - radius1 - radius2

        self.assertLess(get_min_distance_actual(), min_distance - tol)
        self.prog.SetInitialGuess(ik.q(), self.plant.GetPositions(context))
        result = mp.Solve(self.prog)
        self.assertTrue(result.is_success())
        q_val = result.GetSolution(ik.q())
        self.plant.SetPositions(context, q_val)
        self.assertGreater(get_min_distance_actual(), min_distance - tol)

        with catch_drake_warnings(expected_count=2):
            self.assertEqual(
                self.prog.Solve(), mp.SolutionResult.kSolutionFound)
            self.assertTrue(np.allclose(
                self.prog.GetSolution(ik.q()), q_val))

    def test_AddOrientationConstraint(self):
        theta_bound = 0.2 * math.pi
        R_AbarA = RotationMatrix(quaternion=Quaternion(0.5, -0.5, 0.5, 0.5))
        R_BbarB = RotationMatrix(
            quaternion=Quaternion(1.0 / 3, 2.0 / 3, 0, 2.0 / 3))
        self.ik_two_bodies.AddOrientationConstraint(
            frameAbar=self.body1_frame, R_AbarA=R_AbarA,
            frameBbar=self.body2_frame, R_BbarB=R_BbarB,
            theta_bound=theta_bound)

        result = mp.Solve(self.prog)
        self.assertTrue(result.is_success())
        q_val = result.GetSolution(self.q)

        body1_quat = self._body1_quat(q_val)
        body2_quat = self._body2_quat(q_val)
        body1_rotmat = Quaternion(body1_quat).rotation()
        body2_rotmat = Quaternion(body2_quat).rotation()
        R_AbarBbar = body1_rotmat.transpose().dot(body2_rotmat)
        R_AB = R_AbarA.matrix().transpose().dot(
            R_AbarBbar.dot(R_BbarB.matrix()))
        self.assertGreater(R_AB.trace(), 1 + 2 * math.cos(theta_bound) - 1E-6)

        with catch_drake_warnings(expected_count=2):
            self.assertEqual(
                self.prog.Solve(), mp.SolutionResult.kSolutionFound)
            q_val = self.prog.GetSolution(self.q)

            body1_quat = self._body1_quat(q_val)
            body2_quat = self._body2_quat(q_val)
            body1_rotmat = Quaternion(body1_quat).rotation()
            body2_rotmat = Quaternion(body2_quat).rotation()
            R_AbarBbar = body1_rotmat.transpose().dot(body2_rotmat)
            R_AB = R_AbarA.matrix().transpose().dot(
                R_AbarBbar.dot(R_BbarB.matrix()))
            self.assertGreater(R_AB.trace(),
                               1 + 2 * math.cos(theta_bound) - 1E-6)

 def __init__(self):
     LeafSystem_[float].__init__(self)
     with catch_drake_warnings(expected_count=1):
         self.DeclareAbstractInputPort("in")
     self.DeclareVectorOutputPort("out", BasicVector(1), self._Out)

    def test_context_api(self):
        # Capture miscellaneous functions not yet tested.
        model_value = AbstractValue.Make("Hello")
        model_vector = BasicVector([1., 2.])

        class TrivialSystem(LeafSystem):
            def __init__(self):
                LeafSystem.__init__(self)
                self.DeclareContinuousState(1)
                self.DeclareDiscreteState(2)
                self.DeclareAbstractState(model_value.Clone())
                self.DeclareAbstractParameter(model_value.Clone())
                self.DeclareNumericParameter(model_vector.Clone())

        system = TrivialSystem()
        context = system.CreateDefaultContext()
        self.assertTrue(
            context.get_state() is context.get_mutable_state())
        self.assertEqual(context.num_continuous_states(), 1)
        self.assertTrue(
            context.get_continuous_state_vector() is
            context.get_mutable_continuous_state_vector())
        self.assertEqual(context.num_discrete_state_groups(), 1)
        with catch_drake_warnings(expected_count=1):
            context.get_num_discrete_state_groups()
        self.assertTrue(
            context.get_discrete_state_vector() is
            context.get_mutable_discrete_state_vector())
        self.assertTrue(
            context.get_discrete_state(0) is
            context.get_discrete_state_vector())
        self.assertTrue(
            context.get_discrete_state(0) is
            context.get_discrete_state().get_vector(0))
        self.assertTrue(
            context.get_mutable_discrete_state(0) is
            context.get_mutable_discrete_state_vector())
        self.assertTrue(
            context.get_mutable_discrete_state(0) is
            context.get_mutable_discrete_state().get_vector(0))
        self.assertEqual(context.num_abstract_states(), 1)
        with catch_drake_warnings(expected_count=1):
            context.get_num_abstract_states()
        self.assertTrue(
            context.get_abstract_state() is
            context.get_mutable_abstract_state())
        self.assertTrue(
            context.get_abstract_state(0) is
            context.get_mutable_abstract_state(0))
        self.assertEqual(
            context.get_abstract_state(0).get_value(), model_value.get_value())

        # Check abstract state API (also test AbstractValues).
        values = context.get_abstract_state()
        self.assertEqual(values.size(), 1)
        self.assertEqual(
            values.get_value(0).get_value(), model_value.get_value())
        self.assertEqual(
            values.get_mutable_value(0).get_value(), model_value.get_value())
        values.SetFrom(values.Clone())
        with catch_drake_warnings(expected_count=1):
            values.CopyFrom(values.Clone())

        # Check parameter accessors.
        self.assertEqual(system.num_abstract_parameters(), 1)
        self.assertEqual(
            context.get_abstract_parameter(index=0).get_value(),
            model_value.get_value())
        self.assertEqual(system.num_numeric_parameter_groups(), 1)
        np.testing.assert_equal(
            context.get_numeric_parameter(index=0).get_value(),
            model_vector.get_value())

        # Check diagram context accessors.
        builder = DiagramBuilder()
        builder.AddSystem(system)
        diagram = builder.Build()
        context = diagram.CreateDefaultContext()
        # Existence check.
        self.assertIsNot(
            diagram.GetMutableSubsystemState(system, context), None)
        subcontext = diagram.GetMutableSubsystemContext(system, context)
        self.assertIsNot(subcontext, None)
        self.assertIs(
            diagram.GetSubsystemContext(system, context), subcontext)