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)