Python MultibodyPlant.num_actuated_dofs方法代码示例

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