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 main():
num_poses = 3
alpha = np.array([.1, .3, 1])
visualizer = MultiposeVisualizer(FindResourceOrThrow(
"examples/Cassie/urdf/cassie_v2.urdf"), num_poses, alpha, "")
# Create a plant just to know number of positions (also as a pydrake test)
builder = pydrake.systems.framework.DiagramBuilder()
plant, scene_graph = pydrake.multibody.plant.AddMultibodyPlantSceneGraph(
builder, 0)
pydrake.multibody.parsing.Parser(plant).AddModelFromFile(
FindResourceOrThrow("examples/Cassie/urdf/cassie_v2.urdf"))
plant.Finalize()
visualizer.DrawPoses(np.random.rand(plant.num_positions(),
num_poses))
import os
import pydrake.multibody.parsing
import pydrake.multibody.plant
import pydrake.systems.framework
import pydrake.systems.meshcat_visualizer
import manipulation.utils
filename = manipulation.utils.FindResource("models/two_bins_w_cameras.yaml")
builder = pydrake.systems.framework.DiagramBuilder()
plant, scene_graph = pydrake.multibody.plant.AddMultibodyPlantSceneGraph(
builder, time_step=0.001)
parser = pydrake.multibody.parsing.Parser(plant)
manipulation.utils.AddPackagePaths(parser)
pydrake.multibody.parsing.ProcessModelDirectives(
pydrake.multibody.parsing.LoadModelDirectives(filename), plant, parser)
plant.Finalize()
meshcat = pydrake.systems.meshcat_visualizer.ConnectMeshcatVisualizer(
builder, scene_graph, zmq_url="new", open_browser=False)
diagram = builder.Build()
context = diagram.CreateDefaultContext()
meshcat.load()
diagram.Publish(context)