def test_map_qdot_to_v_and_back(self):
     plant = MultibodyPlant()
     iiwa_sdf_path = FindResourceOrThrow(
         "drake/manipulation/models/"
         "iiwa_description/sdf/iiwa14_no_collision.sdf")
     # Use floating base to effectively add a quatnerion in the generalized
     # quaternion.
     iiwa_model = Parser(plant=plant).AddModelFromFile(
         file_name=iiwa_sdf_path, model_name='robot')
     plant.Finalize()
     context = plant.CreateDefaultContext()
     # Try mapping velocity to qdot and back.
     nq = plant.num_positions()
     nv = plant.num_velocities()
     q_init = np.linspace(start=1.0, stop=nq, num=nq)
     plant.SetPositions(context, q_init)
     # Overwrite the (invalid) base coordinates, wherever in `q` they are.
     plant.SetFreeBodyPose(
         context, plant.GetBodyByName("iiwa_link_0"),
         RigidTransform(RollPitchYaw([0.1, 0.2, 0.3]),
                        p=[0.4, 0.5, 0.6]))
     v_expected = np.linspace(start=-1.0, stop=-nv, num=nv)
     qdot = plant.MapVelocityToQDot(context, v_expected)
     v_remap = plant.MapQDotToVelocity(context, qdot)
     self.assertTrue(np.allclose(v_expected, v_remap))

    def test_multibody_state_access(self):
        file_name = FindResourceOrThrow(
            "drake/multibody/benchmarks/acrobot/acrobot.sdf")
        plant = MultibodyPlant()
        Parser(plant).AddModelFromFile(file_name)
        plant.Finalize()
        context = plant.CreateDefaultContext()

        nq = 2
        nv = 2
        self.assertEqual(plant.num_positions(), nq)
        self.assertEqual(plant.num_velocities(), nv)

        q0 = np.array([3.14, 2.])
        v0 = np.array([-0.5, 1.])
        x0 = np.concatenate([q0, v0])

        # The default state is all values set to zero.
        x = plant.GetPositionsAndVelocities(context)
        self.assertTrue(np.allclose(x, np.zeros(4)))

        # Write into a mutable reference to the state vector.
        x_ref = plant.GetMutablePositionsAndVelocities(context)
        x_ref[:] = x0

        # Verify that positions and velocities were set correctly.
        self.assertTrue(np.allclose(plant.GetPositions(context), q0))
        self.assertTrue(np.allclose(plant.GetVelocities(context), v0))

        # Verify we did modify the state stored in context.
        x = plant.GetPositionsAndVelocities(context)
        self.assertTrue(np.allclose(x, x0))

        # Now set positions and velocities independently and check them.
        zeros_2 = np.zeros([2, 1])
        x_ref.fill(0)
        plant.SetPositions(context, q0)
        self.assertTrue(np.allclose(plant.GetPositions(context), q0))
        self.assertTrue(np.allclose(plant.GetVelocities(context), zeros_2))
        x_ref.fill(0)
        plant.SetVelocities(context, v0)
        self.assertTrue(np.allclose(plant.GetPositions(context), zeros_2))
        self.assertTrue(np.allclose(plant.GetVelocities(context), v0))

        # Now test SetPositionsAndVelocities().
        x_ref.fill(0)
        plant.SetPositionsAndVelocities(context, x0)
        self.assertTrue(np.allclose(
            plant.GetPositionsAndVelocities(context), x0))

        # Test existence of context resetting methods.
        plant.SetDefaultState(context, state=context.get_mutable_state())

        # Test existence of limits.
        self.assertEqual(plant.GetPositionLowerLimits().shape, (nq,))
        self.assertEqual(plant.GetPositionUpperLimits().shape, (nq,))
        self.assertEqual(plant.GetVelocityLowerLimits().shape, (nv,))
        self.assertEqual(plant.GetVelocityUpperLimits().shape, (nv,))
        self.assertEqual(plant.GetAccelerationLowerLimits().shape, (nv,))
        self.assertEqual(plant.GetAccelerationUpperLimits().shape, (nv,))

    def test_model_instance_port_access(self):
        # Create a MultibodyPlant with a kuka arm and a schunk gripper.
        # the arm is welded to the world, the gripper is welded to the
        # arm's end effector.
        wsg50_sdf_path = FindResourceOrThrow(
            "drake/manipulation/models/" +
            "wsg_50_description/sdf/schunk_wsg_50.sdf")
        iiwa_sdf_path = FindResourceOrThrow(
            "drake/manipulation/models/" +
            "iiwa_description/sdf/iiwa14_no_collision.sdf")

        plant = MultibodyPlant(time_step=2e-3)
        parser = Parser(plant)
        iiwa_model = parser.AddModelFromFile(
            file_name=iiwa_sdf_path, model_name='robot')
        gripper_model = parser.AddModelFromFile(
            file_name=wsg50_sdf_path, model_name='gripper')
        plant.Finalize()

        # Test that we can get an actuation input port and a continuous state
        # output port.
        self.assertIsInstance(
            plant.get_actuation_input_port(iiwa_model), InputPort)
        self.assertIsInstance(
            plant.get_state_output_port(gripper_model), OutputPort)
        self.assertIsInstance(
            plant.get_generalized_contact_forces_output_port(
                model_instance=gripper_model),
            OutputPort)

    def test_multibody_dynamics(self):
        file_name = FindResourceOrThrow(
            "drake/multibody/benchmarks/acrobot/acrobot.sdf")
        plant = MultibodyPlant()
        Parser(plant).AddModelFromFile(file_name)
        plant.Finalize()
        context = plant.CreateDefaultContext()

        H = plant.CalcMassMatrixViaInverseDynamics(context)
        Cv = plant.CalcBiasTerm(context)

        self.assertTrue(H.shape == (2, 2))
        self.assert_sane(H)
        self.assertTrue(Cv.shape == (2, ))
        self.assert_sane(Cv, nonzero=False)
        nv = plant.num_velocities()
        vd_d = np.zeros(nv)
        tau = plant.CalcInverseDynamics(
            context, vd_d, MultibodyForces(plant))
        self.assertEqual(tau.shape, (2,))
        self.assert_sane(tau, nonzero=False)
        # - Existence checks.
        self.assertEqual(plant.CalcPotentialEnergy(context), 0)
        plant.CalcConservativePower(context)
        tau_g = plant.CalcGravityGeneralizedForces(context)
        self.assertEqual(tau_g.shape, (nv,))
        self.assert_sane(tau_g, nonzero=False)

        forces = MultibodyForces(plant=plant)
        plant.CalcForceElementsContribution(context=context, forces=forces)

 def test_multibody_add_frame(self):
     plant = MultibodyPlant()
     frame = plant.AddFrame(frame=FixedOffsetFrame(
         name="frame", P=plant.world_frame(),
         X_PF=RigidTransform.Identity(), model_instance=None))
     self.assertIsInstance(frame, Frame)
     np.testing.assert_equal(
         np.eye(4), frame.GetFixedPoseInBodyFrame().GetAsMatrix4())

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

        with warnings.catch_warnings(record=True) as w:
            warnings.simplefilter('always', DrakeDeprecationWarning)
            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_inverse_dynamics(self):
        sdf_path = FindResourceOrThrow(
            "drake/manipulation/models/" +
            "iiwa_description/sdf/iiwa14_no_collision.sdf")

        plant = MultibodyPlant(time_step=0.01)
        Parser(plant).AddModelFromFile(sdf_path)
        plant.WeldFrames(plant.world_frame(),
                         plant.GetFrameByName("iiwa_link_0"))
        plant.Finalize()

        # Just test that the constructor doesn't throw.
        controller = InverseDynamics(
            plant=plant,
            mode=InverseDynamics.InverseDynamicsMode.kGravityCompensation)

    def test_multibody_dynamics(self):
        file_name = FindResourceOrThrow(
            "drake/multibody/benchmarks/acrobot/acrobot.sdf")
        plant = MultibodyPlant()
        Parser(plant).AddModelFromFile(file_name)
        # Getting ready for when we set foot on Mars :-).
        plant.AddForceElement(UniformGravityFieldElement([0.0, 0.0, -3.71]))
        plant.Finalize()
        context = plant.CreateDefaultContext()

        # Set an arbitrary configuration away from the model's fixed point.
        plant.SetPositions(context, [0.1, 0.2])

        H = plant.CalcMassMatrixViaInverseDynamics(context)
        Cv = plant.CalcBiasTerm(context)

        self.assertTrue(H.shape == (2, 2))
        self.assert_sane(H)
        self.assertTrue(Cv.shape == (2, ))
        self.assert_sane(Cv, nonzero=False)
        nv = plant.num_velocities()
        vd_d = np.zeros(nv)
        tau = plant.CalcInverseDynamics(
            context, vd_d, MultibodyForces(plant))
        self.assertEqual(tau.shape, (2,))
        self.assert_sane(tau, nonzero=False)
        # - Existence checks.
        # Gravity leads to non-zero potential energy.
        self.assertNotEqual(plant.CalcPotentialEnergy(context), 0)
        plant.CalcConservativePower(context)
        tau_g = plant.CalcGravityGeneralizedForces(context)
        self.assertEqual(tau_g.shape, (nv,))
        self.assert_sane(tau_g, nonzero=True)

        forces = MultibodyForces(plant=plant)
        plant.CalcForceElementsContribution(context=context, forces=forces)

    def test_multibody_dynamics(self):
        file_name = FindResourceOrThrow(
            "drake/multibody/benchmarks/acrobot/acrobot.sdf")
        plant = MultibodyPlant()
        Parser(plant).AddModelFromFile(file_name)
        # Getting ready for when we set foot on Mars :-).
        gravity_vector = np.array([0.0, 0.0, -3.71])
        plant.mutable_gravity_field().set_gravity_vector(gravity_vector)
        np.testing.assert_equal(plant.gravity_field().gravity_vector(),
                                gravity_vector)
        plant.Finalize()
        context = plant.CreateDefaultContext()

        # Set an arbitrary configuration away from the model's fixed point.
        plant.SetPositions(context, [0.1, 0.2])

        M = plant.CalcMassMatrixViaInverseDynamics(context)
        Cv = plant.CalcBiasTerm(context)

        self.assertTrue(M.shape == (2, 2))
        self.assert_sane(M)
        self.assertTrue(Cv.shape == (2, ))
        self.assert_sane(Cv, nonzero=False)
        nv = plant.num_velocities()
        vd_d = np.zeros(nv)
        tau = plant.CalcInverseDynamics(
            context, vd_d, MultibodyForces(plant))
        self.assertEqual(tau.shape, (2,))
        self.assert_sane(tau, nonzero=False)
        # - Existence checks.
        # Gravity leads to non-zero potential energy.
        self.assertNotEqual(plant.CalcPotentialEnergy(context), 0)
        plant.CalcConservativePower(context)
        tau_g = plant.CalcGravityGeneralizedForces(context)
        self.assertEqual(tau_g.shape, (nv,))
        self.assert_sane(tau_g, nonzero=True)

        B = plant.MakeActuationMatrix()
        np.testing.assert_equal(B, np.array([[0.], [1.]]))

        forces = MultibodyForces(plant=plant)
        plant.CalcForceElementsContribution(context=context, forces=forces)

        # Test generalized forces.
        forces.mutable_generalized_forces()[:] = 1
        np.testing.assert_equal(forces.generalized_forces(), 1)
        forces.SetZero()
        np.testing.assert_equal(forces.generalized_forces(), 0)
        # Test body force accessors and mutators.
        link2 = plant.GetBodyByName("Link2")
        self.assertIsInstance(
            link2.GetForceInWorld(context, forces), SpatialForce)
        forces.SetZero()
        F_expected = np.array([1, 2, 3, 4, 5, 6])
        link2.AddInForceInWorld(
            context, F_Bo_W=SpatialForce(F=F_expected), forces=forces)
        np.testing.assert_equal(
            link2.GetForceInWorld(context, forces).get_coeffs(), F_expected)
        link2.AddInForce(
            context, p_BP_E=[0, 0, 0], F_Bp_E=SpatialForce(F=F_expected),
            frame_E=plant.world_frame(), forces=forces)
        # Also check accumulation.
        np.testing.assert_equal(
            link2.GetForceInWorld(context, forces).get_coeffs(),
            2 * F_expected)

    def test_model_instance_state_access_by_array(self):
        # Create a MultibodyPlant with a kuka arm and a schunk gripper.
        # the arm is welded to the world, the gripper is welded to the
        # arm's end effector.
        wsg50_sdf_path = FindResourceOrThrow(
            "drake/manipulation/models/" +
            "wsg_50_description/sdf/schunk_wsg_50.sdf")
        iiwa_sdf_path = FindResourceOrThrow(
            "drake/manipulation/models/" +
            "iiwa_description/sdf/iiwa14_no_collision.sdf")

        timestep = 0.0002
        plant = MultibodyPlant(timestep)
        parser = Parser(plant)

        iiwa_model = parser.AddModelFromFile(
            file_name=iiwa_sdf_path, model_name='robot')
        gripper_model = parser.AddModelFromFile(
            file_name=wsg50_sdf_path, model_name='gripper')

        # Weld the base of arm and gripper to reduce the number of states.
        X_EeGripper = RigidTransform(
            RollPitchYaw(np.pi / 2, 0, np.pi / 2), [0, 0, 0.081])
        plant.WeldFrames(
            A=plant.world_frame(),
            B=plant.GetFrameByName("iiwa_link_0", iiwa_model))
        plant.WeldFrames(
            A=plant.GetFrameByName("iiwa_link_7", iiwa_model),
            B=plant.GetFrameByName("body", gripper_model),
            X_AB=X_EeGripper)
        plant.Finalize()

        # Create a context of the MBP and set the state of the context
        # to desired values.
        context = plant.CreateDefaultContext()

        nq = plant.num_positions()
        nq_iiwa = plant.num_positions(iiwa_model)
        nv = plant.num_velocities()
        nv_iiwa = plant.num_velocities(iiwa_model)

        q_iiwa_desired = np.linspace(0, 0.3, 7)
        v_iiwa_desired = q_iiwa_desired + 0.4
        q_gripper_desired = [0.4, 0.5]
        v_gripper_desired = [-1., -2.]

        x_desired = np.zeros(nq + nv)
        x_desired[0:7] = q_iiwa_desired
        x_desired[7:9] = q_gripper_desired
        x_desired[nq:nq+7] = v_iiwa_desired
        x_desired[nq+7:nq+nv] = v_gripper_desired

        x = plant.GetMutablePositionsAndVelocities(context=context)
        x[:] = x_desired
        q = plant.GetPositions(context=context)
        v = plant.GetVelocities(context=context)

        # Get state from context.
        x = plant.GetPositionsAndVelocities(context=context)
        x_tmp = plant.GetMutablePositionsAndVelocities(context=context)
        self.assertTrue(np.allclose(x_desired, x_tmp))

        # Get positions and velocities of specific model instances
        # from the position/velocity vector of the plant.
        q_iiwa = plant.GetPositions(context=context, model_instance=iiwa_model)
        q_iiwa_array = plant.GetPositionsFromArray(
            model_instance=iiwa_model, q=q)
        self.assertTrue(np.allclose(q_iiwa, q_iiwa_array))
        q_gripper = plant.GetPositions(
            context=context, model_instance=gripper_model)
        v_iiwa = plant.GetVelocities(
            context=context, model_instance=iiwa_model)
        v_iiwa_array = plant.GetVelocitiesFromArray(
            model_instance=iiwa_model, v=v)
        self.assertTrue(np.allclose(v_iiwa, v_iiwa_array))
        v_gripper = plant.GetVelocities(
            context=context, model_instance=gripper_model)

        # Assert that the `GetPositions` and `GetVelocities` return
        # the desired values set earlier.
        self.assertTrue(np.allclose(q_iiwa_desired, q_iiwa))
        self.assertTrue(np.allclose(v_iiwa_desired, v_iiwa))
        self.assertTrue(np.allclose(q_gripper_desired, q_gripper))
        self.assertTrue(np.allclose(v_gripper_desired, v_gripper))

        # Verify that SetPositionsInArray() and SetVelocitiesInArray() works.
        plant.SetPositionsInArray(
            model_instance=iiwa_model, q_instance=np.zeros(nq_iiwa), q=q)
        self.assertTrue(np.allclose(
            plant.GetPositionsFromArray(model_instance=iiwa_model, q=q),
            np.zeros(nq_iiwa)))
        plant.SetVelocitiesInArray(
            model_instance=iiwa_model, v_instance=np.zeros(nv_iiwa), v=v)
        self.assertTrue(np.allclose(
            plant.GetVelocitiesFromArray(model_instance=iiwa_model, v=v),
            np.zeros(nv_iiwa)))

        # Check actuation.
        nu = plant.num_actuated_dofs()
        u = np.zeros(nu)
#.........这里部分代码省略.........

    def test_model_instance_state_access(self):
        # Create a MultibodyPlant with a kuka arm and a schunk gripper.
        # the arm is welded to the world, the gripper is welded to the
        # arm's end effector.
        wsg50_sdf_path = FindResourceOrThrow(
            "drake/manipulation/models/" +
            "wsg_50_description/sdf/schunk_wsg_50.sdf")
        iiwa_sdf_path = FindResourceOrThrow(
            "drake/manipulation/models/" +
            "iiwa_description/sdf/iiwa14_no_collision.sdf")

        plant = MultibodyPlant()
        parser = Parser(plant)

        iiwa_model = parser.AddModelFromFile(
            file_name=iiwa_sdf_path, model_name='robot')
        gripper_model = parser.AddModelFromFile(
            file_name=wsg50_sdf_path, model_name='gripper')

        # Weld the base of arm and gripper to reduce the number of states.
        X_EeGripper = RigidTransform(
            RollPitchYaw(np.pi / 2, 0, np.pi / 2), [0, 0, 0.081])
        plant.WeldFrames(A=plant.world_frame(),
                         B=plant.GetFrameByName("iiwa_link_0", iiwa_model))
        plant.WeldFrames(
            A=plant.GetFrameByName("iiwa_link_7", iiwa_model),
            B=plant.GetFrameByName("body", gripper_model),
            X_AB=X_EeGripper)
        plant.Finalize()

        # Create a context of the MBP and set the state of the context
        # to desired values.
        context = plant.CreateDefaultContext()

        nq = plant.num_positions()
        nv = plant.num_velocities()

        nq_iiwa = 7
        nv_iiwa = 7
        nq_gripper = 2
        nv_gripper = 2
        q_iiwa_desired = np.zeros(nq_iiwa)
        v_iiwa_desired = np.zeros(nv_iiwa)
        q_gripper_desired = np.zeros(nq_gripper)
        v_gripper_desired = np.zeros(nv_gripper)

        q_iiwa_desired[2] = np.pi/3
        q_gripper_desired[0] = 0.1
        v_iiwa_desired[1] = 5.0
        q_gripper_desired[0] = -0.3

        x_iiwa_desired = np.zeros(nq_iiwa + nv_iiwa)
        x_iiwa_desired[0:nq_iiwa] = q_iiwa_desired
        x_iiwa_desired[nq_iiwa:nq_iiwa+nv_iiwa] = v_iiwa_desired

        x_gripper_desired = np.zeros(nq_gripper + nv_gripper)
        x_gripper_desired[0:nq_gripper] = q_gripper_desired
        x_gripper_desired[nq_gripper:nq_gripper+nv_gripper] = v_gripper_desired

        x_desired = np.zeros(nq + nv)
        x_desired[0:7] = q_iiwa_desired
        x_desired[7:9] = q_gripper_desired
        x_desired[nq:nq+7] = v_iiwa_desired
        x_desired[nq+7:nq+nv] = v_gripper_desired

        # Check SetPositionsAndVelocities() for each model instance.
        # Do the iiwa model first.
        plant.SetPositionsAndVelocities(context, np.zeros(nq + nv))
        self.assertTrue(np.allclose(plant.GetPositionsAndVelocities(context),
                                    np.zeros(nq + nv)))
        plant.SetPositionsAndVelocities(context, iiwa_model, x_iiwa_desired)
        self.assertTrue(np.allclose(
            plant.GetPositionsAndVelocities(context, iiwa_model),
            x_iiwa_desired))
        self.assertTrue(np.allclose(plant.GetPositionsAndVelocities(
            context, gripper_model), np.zeros(nq_gripper + nv_gripper)))
        # Do the gripper model.
        plant.SetPositionsAndVelocities(context, np.zeros(nq + nv))
        self.assertTrue(np.allclose(plant.GetPositionsAndVelocities(context),
                                    np.zeros(nq + nv)))
        plant.SetPositionsAndVelocities(
            context, gripper_model, x_gripper_desired)
        self.assertTrue(np.allclose(
            plant.GetPositionsAndVelocities(context, gripper_model),
            x_gripper_desired))
        self.assertTrue(np.allclose(plant.GetPositionsAndVelocities(
            context, iiwa_model), np.zeros(nq_iiwa + nv_iiwa)))

        # Check SetPositions() for each model instance.
        # Do the iiwa model first.
        plant.SetPositionsAndVelocities(context, np.zeros(nq + nv))
        self.assertTrue(np.allclose(plant.GetPositionsAndVelocities(context),
                                    np.zeros(nq + nv)))
        plant.SetPositions(context, iiwa_model, q_iiwa_desired)
        self.assertTrue(np.allclose(
            plant.GetPositions(context, iiwa_model), q_iiwa_desired))
        self.assertTrue(np.allclose(plant.GetVelocities(
            context, iiwa_model), np.zeros(nv_iiwa)))
        self.assertTrue(np.allclose(plant.GetPositionsAndVelocities(
            context, gripper_model), np.zeros(nq_gripper + nv_gripper)))
#.........这里部分代码省略.........

    def test_multibody_tree_kinematics(self):
        file_name = FindResourceOrThrow(
            "drake/examples/double_pendulum/models/double_pendulum.sdf")
        plant = MultibodyPlant()
        Parser(plant).AddModelFromFile(file_name)
        plant.Finalize()
        context = plant.CreateDefaultContext()
        world_frame = plant.world_frame()
        base = plant.GetBodyByName("base")
        base_frame = plant.GetFrameByName("base")
        X_WL = plant.CalcRelativeTransform(
            context, frame_A=world_frame, frame_B=base_frame)
        self.assertIsInstance(X_WL, RigidTransform)

        p_AQi = plant.CalcPointsPositions(
            context=context, frame_B=base_frame,
            p_BQi=np.array([[0, 1, 2], [10, 11, 12]]).T,
            frame_A=world_frame).T
        self.assertTupleEqual(p_AQi.shape, (2, 3))

        Jv_WL = plant.CalcFrameGeometricJacobianExpressedInWorld(
            context=context, frame_B=base_frame,
            p_BoFo_B=[0, 0, 0])
        self.assertTupleEqual(Jv_WL.shape, (6, plant.num_velocities()))

        nq = plant.num_positions()
        nv = plant.num_velocities()
        wrt_list = [
            (JacobianWrtVariable.kQDot, nq),
            (JacobianWrtVariable.kV, nv),
        ]
        for wrt, nw in wrt_list:
            Jw_ABp_E = plant.CalcJacobianSpatialVelocity(
                context=context, with_respect_to=wrt, frame_B=base_frame,
                p_BP=np.zeros(3), frame_A=world_frame,
                frame_E=world_frame)
            self.assert_sane(Jw_ABp_E)
            self.assertEqual(Jw_ABp_E.shape, (6, nw))

        # Compute body pose.
        X_WBase = plant.EvalBodyPoseInWorld(context, base)
        self.assertIsInstance(X_WBase, RigidTransform)

        # Set pose for the base.
        X_WB_desired = RigidTransform.Identity()
        X_WB = plant.CalcRelativeTransform(context, world_frame, base_frame)
        plant.SetFreeBodyPose(
            context=context, body=base, X_WB=X_WB_desired)
        self.assertTrue(np.allclose(X_WB.matrix(), X_WB_desired.matrix()))

        # Set a spatial velocity for the base.
        v_WB = SpatialVelocity(w=[1, 2, 3], v=[4, 5, 6])
        plant.SetFreeBodySpatialVelocity(
            context=context, body=base, V_WB=v_WB)
        v_base = plant.EvalBodySpatialVelocityInWorld(context, base)
        self.assertTrue(np.allclose(v_base.rotational(), v_WB.rotational()))
        self.assertTrue(np.allclose(v_base.translational(),
                                    v_WB.translational()))

        # Compute accelerations.
        vdot = np.zeros(nv)
        A_WB_array = plant.CalcSpatialAccelerationsFromVdot(
            context=context, known_vdot=vdot)
        self.assertEqual(len(A_WB_array), plant.num_bodies())

 def test_multibody_plant_api_via_parsing(self):
     # TODO(eric.cousineau): Decouple this when construction can be done
     # without parsing.
     # This a subset of `multibody_plant_sdf_parser_test.cc`.
     file_name = FindResourceOrThrow(
         "drake/multibody/benchmarks/acrobot/acrobot.sdf")
     plant = MultibodyPlant(time_step=0.01)
     model_instance = Parser(plant).AddModelFromFile(file_name)
     self.assertIsInstance(model_instance, ModelInstanceIndex)
     plant.Finalize()
     benchmark = MakeAcrobotPlant(AcrobotParameters(), True)
     self.assertEqual(plant.num_bodies(), benchmark.num_bodies())
     self.assertEqual(plant.num_joints(), benchmark.num_joints())
     self.assertEqual(plant.num_actuators(), benchmark.num_actuators())
     self.assertEqual(
         plant.num_model_instances(), benchmark.num_model_instances() + 1)
     self.assertEqual(plant.num_positions(), benchmark.num_positions())
     self.assertEqual(
         plant.num_positions(model_instance=model_instance),
         benchmark.num_positions())
     self.assertEqual(
         plant.num_velocities(), benchmark.num_velocities())
     self.assertEqual(
         plant.num_multibody_states(), benchmark.num_multibody_states())
     self.assertEqual(
         plant.num_actuated_dofs(), benchmark.num_actuated_dofs())
     self.assertTrue(plant.is_finalized())
     self.assertTrue(plant.HasBodyNamed(name="Link1"))
     self.assertTrue(plant.HasBodyNamed(
         name="Link1", model_instance=model_instance))
     self.assertTrue(plant.HasJointNamed(name="ShoulderJoint"))
     self.assertTrue(plant.HasJointNamed(
         name="ShoulderJoint", model_instance=model_instance))
     shoulder = plant.GetJointByName(name="ShoulderJoint")
     self._test_joint_api(shoulder)
     np.testing.assert_array_equal(
         shoulder.position_lower_limits(), [-np.inf])
     np.testing.assert_array_equal(
         shoulder.position_upper_limits(), [np.inf])
     self.assertIs(shoulder, plant.GetJointByName(
         name="ShoulderJoint", model_instance=model_instance))
     self._test_joint_actuator_api(
         plant.GetJointActuatorByName(name="ElbowJoint"))
     self._test_body_api(plant.GetBodyByName(name="Link1"))
     self.assertIs(
         plant.GetBodyByName(name="Link1"),
         plant.GetBodyByName(name="Link1", model_instance=model_instance))
     self.assertEqual(len(plant.GetBodyIndices(model_instance)), 2)
     self._test_frame_api(plant.GetFrameByName(name="Link1"))
     self.assertIs(
         plant.GetFrameByName(name="Link1"),
         plant.GetFrameByName(name="Link1", model_instance=model_instance))
     self.assertEqual(
         model_instance, plant.GetModelInstanceByName(name="acrobot"))
     self.assertIsInstance(
         plant.get_actuation_input_port(), InputPort)
     self.assertIsInstance(
         plant.get_state_output_port(), OutputPort)
     # Smoke test of deprecated methods.
     with catch_drake_warnings(expected_count=2):
         plant.get_continuous_state_output_port()
         plant.get_continuous_state_output_port(model_instance)
     self.assertIsInstance(
         plant.get_contact_results_output_port(), OutputPort)
     self.assertIsInstance(plant.num_frames(), int)
     self.assertIsInstance(plant.get_body(body_index=BodyIndex(0)), Body)
     self.assertIs(shoulder, plant.get_joint(joint_index=JointIndex(0)))
     self.assertIsInstance(plant.get_joint_actuator(
         actuator_index=JointActuatorIndex(0)), JointActuator)
     self.assertIsInstance(
         plant.get_frame(frame_index=FrameIndex(0)), Frame)
     self.assertEqual("acrobot", plant.GetModelInstanceName(
         model_instance=model_instance))

    def test_inverse_dynamics_controller(self):
        sdf_path = FindResourceOrThrow(
            "drake/manipulation/models/" +
            "iiwa_description/sdf/iiwa14_no_collision.sdf")

        plant = MultibodyPlant(time_step=0.01)
        Parser(plant).AddModelFromFile(sdf_path)
        plant.WeldFrames(plant.world_frame(),
                         plant.GetFrameByName("iiwa_link_0"))
        plant.Finalize()

        # We verify the (known) size of the model.
        kNumPositions = 7
        kNumVelocities = 7
        kNumActuators = 7
        kStateSize = kNumPositions + kNumVelocities
        self.assertEqual(plant.num_positions(), kNumPositions)
        self.assertEqual(plant.num_velocities(), kNumVelocities)
        self.assertEqual(plant.num_actuators(), kNumActuators)

        kp = np.array([1., 2., 3., 4., 5., 6., 7.])
        ki = np.array([0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7])
        kd = np.array([.5, 1., 1.5, 2., 2.5, 3., 3.5])

        controller = InverseDynamicsController(robot=plant,
                                               kp=kp,
                                               ki=ki,
                                               kd=kd,
                                               has_reference_acceleration=True)
        context = controller.CreateDefaultContext()
        output = controller.AllocateOutput()

        estimated_state_port = 0
        desired_state_port = 1
        desired_acceleration_port = 2
        control_port = 0

        self.assertEqual(
            controller.get_input_port(desired_acceleration_port).size(),
            kNumVelocities)
        self.assertEqual(
            controller.get_input_port(estimated_state_port).size(), kStateSize)
        self.assertEqual(
            controller.get_input_port(desired_state_port).size(), kStateSize)
        self.assertEqual(
            controller.get_output_port(control_port).size(), kNumVelocities)

        # Current state.
        q = np.array([-0.3, -0.2, -0.1, 0.0, 0.1, 0.2, 0.3])
        v = np.array([-0.9, -0.6, -0.3, 0.0, 0.3, 0.6, 0.9])
        x = np.concatenate([q, v])

        # Reference state and acceleration.
        q_r = q + 0.1*np.ones_like(q)
        v_r = v + 0.1*np.ones_like(v)
        x_r = np.concatenate([q_r, v_r])
        vd_r = np.array([1., 2., 3., 4., 5., 6., 7.])

        integral_term = np.array([-1., -2., -3., -4., -5., -6., -7.])

        vd_d = vd_r + kp*(q_r-q) + kd*(v_r-v) + ki*integral_term

        context.FixInputPort(estimated_state_port, BasicVector(x))
        context.FixInputPort(desired_state_port, BasicVector(x_r))
        context.FixInputPort(desired_acceleration_port, BasicVector(vd_r))
        controller.set_integral_value(context, integral_term)

        # Set the plant's context.
        plant_context = plant.CreateDefaultContext()
        x_plant = plant.GetMutablePositionsAndVelocities(plant_context)
        x_plant[:] = x

        # Compute the expected value of the generalized forces using
        # inverse dynamics.
        tau_id = plant.CalcInverseDynamics(
            plant_context, vd_d, MultibodyForces(plant))

        # Verify the result.
        controller.CalcOutput(context, output)
        self.assertTrue(np.allclose(output.get_vector_data(0).CopyToVector(),
                                    tau_id))

    def test_contact_force(self):
        """A block sitting on a table."""
        object_file_path = FindResourceOrThrow(
            "drake/examples/manipulation_station/models/061_foam_brick.sdf")
        table_file_path = FindResourceOrThrow(
            "drake/examples/kuka_iiwa_arm/models/table/"
            "extra_heavy_duty_table_surface_only_collision.sdf")

        # T: tabletop frame.
        X_TObject = RigidTransform([0, 0, 0.2])

        builder = DiagramBuilder()
        plant = MultibodyPlant(0.002)
        _, scene_graph = AddMultibodyPlantSceneGraph(builder, plant)
        object_model = Parser(plant=plant).AddModelFromFile(object_file_path)
        table_model = Parser(plant=plant).AddModelFromFile(table_file_path)

        # Weld table to world.
        plant.WeldFrames(
            A=plant.world_frame(),
            B=plant.GetFrameByName("link", table_model))

        plant.Finalize()

        # Add meshcat visualizer.
        viz = builder.AddSystem(
            MeshcatVisualizer(scene_graph,
                              zmq_url=ZMQ_URL,
                              open_browser=False))
        builder.Connect(
            scene_graph.get_pose_bundle_output_port(),
            viz.get_input_port(0))

        # Add contact visualizer.
        contact_viz = builder.AddSystem(
            MeshcatContactVisualizer(
                meshcat_viz=viz,
                force_threshold=0,
                contact_force_scale=10,
                plant=plant))
        contact_input_port = contact_viz.GetInputPort("contact_results")
        builder.Connect(
            plant.GetOutputPort("contact_results"),
            contact_input_port)
        builder.Connect(
            scene_graph.get_pose_bundle_output_port(),
            contact_viz.GetInputPort("pose_bundle"))

        diagram = builder.Build()

        diagram_context = diagram.CreateDefaultContext()
        mbp_context = diagram.GetMutableSubsystemContext(
            plant, diagram_context)

        X_WT = plant.CalcRelativeTransform(
            mbp_context,
            plant.world_frame(),
            plant.GetFrameByName("top_center"))

        plant.SetFreeBodyPose(
            mbp_context,
            plant.GetBodyByName("base_link", object_model),
            X_WT.multiply(X_TObject))

        simulator = Simulator(diagram, diagram_context)
        simulator.set_publish_every_time_step(False)
        simulator.AdvanceTo(1.0)

        contact_viz_context = (
            diagram.GetMutableSubsystemContext(contact_viz, diagram_context))
        contact_results = contact_viz.EvalAbstractInput(
            contact_viz_context,
            contact_input_port.get_index()).get_value()

        self.assertGreater(contact_results.num_contacts(), 0)
        self.assertEqual(contact_viz._contact_key_counter, 4)

def main():
    args_parser = argparse.ArgumentParser(
        description=__doc__,
        formatter_class=argparse.RawDescriptionHelpFormatter)
    args_parser.add_argument(
        "filename", type=str,
        help="Path to an SDF or URDF file.")
    args_parser.add_argument(
        "--package_path",
        type=str,
        default=None,
        help="Full path to the root package for reading in SDF resources.")
    position_group = args_parser.add_mutually_exclusive_group()
    position_group.add_argument(
        "--position", type=float, nargs="+", default=[],
        help="A list of positions which must be the same length as the number "
             "of positions in the sdf model.  Note that most models have a "
             "floating-base joint by default (unless the sdf explicitly welds "
             "the base to the world, and so have 7 positions corresponding to "
             "the quaternion representation of that floating-base position.")
    position_group.add_argument(
        "--joint_position", type=float, nargs="+", default=[],
        help="A list of positions which must be the same length as the number "
             "of positions ASSOCIATED WITH JOINTS in the sdf model.  This "
             "does not include, e.g., floating-base coordinates, which will "
             "be assigned a default value.")
    args_parser.add_argument(
        "--test", action='store_true',
        help="Disable opening the gui window for testing.")
    # TODO(russt): Add option to weld the base to the world pending the
    # availability of GetUniqueBaseBody requested in #9747.
    args = args_parser.parse_args()
    filename = args.filename
    if not os.path.isfile(filename):
        args_parser.error("File does not exist: {}".format(filename))

    builder = DiagramBuilder()
    scene_graph = builder.AddSystem(SceneGraph())

    # Construct a MultibodyPlant.
    plant = MultibodyPlant()
    plant.RegisterAsSourceForSceneGraph(scene_graph)

    # Create the parser.
    parser = Parser(plant)

    # Get the package pathname.
    if args.package_path:
        # Verify that package.xml is found in the designated path.
        package_path = os.path.abspath(args.package_path)
        if not os.path.isfile(os.path.join(package_path, "package.xml")):
            parser.error("package.xml not found at: {}".format(package_path))

        # Get the package map and populate it using the package path.
        package_map = parser.package_map()
        package_map.PopulateFromFolder(package_path)

    # Add the model from the file and finalize the plant.
    parser.AddModelFromFile(filename)
    plant.Finalize(scene_graph)

    # Add sliders to set positions of the joints.
    sliders = builder.AddSystem(JointSliders(robot=plant))
    to_pose = builder.AddSystem(MultibodyPositionToGeometryPose(plant))
    builder.Connect(sliders.get_output_port(0), to_pose.get_input_port())
    builder.Connect(
        to_pose.get_output_port(),
        scene_graph.get_source_pose_port(plant.get_source_id()))

    # Connect this to drake_visualizer.
    ConnectDrakeVisualizer(builder=builder, scene_graph=scene_graph)

    if len(args.position):
        sliders.set_position(args.position)
    elif len(args.joint_position):
        sliders.set_joint_position(args.joint_position)

    # Make the diagram and run it.
    diagram = builder.Build()
    simulator = Simulator(diagram)

    simulator.set_publish_every_time_step(False)

    if args.test:
        sliders.window.withdraw()
        simulator.AdvanceTo(0.1)
    else:
        simulator.set_target_realtime_rate(1.0)
        simulator.AdvanceTo(np.inf)