Python MultibodyPlant.world_frame方法代码示例

# 需要导入模块: from pydrake.multibody.plant import MultibodyPlant [as 别名]
# 或者: from pydrake.multibody.plant.MultibodyPlant import world_frame [as 别名]
 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())

# 需要导入模块: from pydrake.multibody.plant import MultibodyPlant [as 别名]
# 或者: from pydrake.multibody.plant.MultibodyPlant import world_frame [as 别名]
    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)

# 需要导入模块: from pydrake.multibody.plant import MultibodyPlant [as 别名]
# 或者: from pydrake.multibody.plant.MultibodyPlant import world_frame [as 别名]
    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)

# 需要导入模块: from pydrake.multibody.plant import MultibodyPlant [as 别名]
# 或者: from pydrake.multibody.plant.MultibodyPlant import world_frame [as 别名]
    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)
#.........这里部分代码省略.........

# 需要导入模块: from pydrake.multibody.plant import MultibodyPlant [as 别名]
# 或者: from pydrake.multibody.plant.MultibodyPlant import world_frame [as 别名]
    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)))
#.........这里部分代码省略.........

# 需要导入模块: from pydrake.multibody.plant import MultibodyPlant [as 别名]
# 或者: from pydrake.multibody.plant.MultibodyPlant import world_frame [as 别名]
    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())

# 需要导入模块: from pydrake.multibody.plant import MultibodyPlant [as 别名]
# 或者: from pydrake.multibody.plant.MultibodyPlant import world_frame [as 别名]
    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))

# 需要导入模块: from pydrake.multibody.plant import MultibodyPlant [as 别名]
# 或者: from pydrake.multibody.plant.MultibodyPlant import world_frame [as 别名]
    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)