本文整理汇总了Python中pydrake.multibody.plant.MultibodyPlant.num_bodies方法的典型用法代码示例。如果您正苦于以下问题：Python MultibodyPlant.num_bodies方法的具体用法？Python MultibodyPlant.num_bodies怎么用？Python MultibodyPlant.num_bodies使用的例子？那么, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类pydrake.multibody.plant.MultibodyPlant的用法示例。
在下文中一共展示了MultibodyPlant.num_bodies方法的2个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的Python代码示例。
示例1: test_multibody_tree_kinematics
# 需要导入模块: from pydrake.multibody.plant import MultibodyPlant [as 别名]
# 或者: from pydrake.multibody.plant.MultibodyPlant import num_bodies [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())示例2: test_multibody_plant_api_via_parsing
# 需要导入模块: from pydrake.multibody.plant import MultibodyPlant [as 别名]
# 或者: from pydrake.multibody.plant.MultibodyPlant import num_bodies [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))