def setupPendulumExample():
rbt = RigidBodyTree(FindResource("pendulum/pendulum.urdf"),
floating_base_type=FloatingBaseType.kFixed) # noqa
Tview = np.array([[1., 0., 0., 0.], [0., 0., 1., 0.], [0., 0., 0., 1.]],
dtype=np.float64)
pbrv = PlanarRigidBodyVisualizer(rbt, Tview, [-1.2, 1.2], [-1.2, 1.2])
return rbt, pbrv
def setupValkyrieExample():
# Valkyrie Example
rbt = RigidBodyTree()
world_frame = RigidBodyFrame("world_frame", rbt.world(), [0, 0, 0],
[0, 0, 0])
from pydrake.multibody.parsers import PackageMap
pmap = PackageMap()
# Note: Val model is currently not installed in drake binary distribution.
pmap.PopulateFromFolder(os.path.join(pydrake.getDrakePath(), "examples"))
# TODO(russt): remove plane.urdf and call AddFlatTerrainTOWorld instead
AddModelInstanceFromUrdfStringSearchingInRosPackages(
open(FindResource(os.path.join("underactuated", "plane.urdf")),
'r').read(), # noqa
pmap,
pydrake.getDrakePath() + "/examples/",
FloatingBaseType.kFixed,
world_frame,
rbt)
val_start_frame = RigidBodyFrame("val_start_frame", rbt.world(),
[0, 0, 1.5], [0, 0, 0])
AddModelInstanceFromUrdfStringSearchingInRosPackages(
open(
pydrake.getDrakePath() +
"/examples/valkyrie/urdf/urdf/valkyrie_A_sim_drake_one_neck_dof_wide_ankle_rom.urdf",
'r').read(), # noqa
pmap,
pydrake.getDrakePath() + "/examples/",
FloatingBaseType.kRollPitchYaw,
val_start_frame,
rbt)
Tview = np.array([[1., 0., 0., 0.], [0., 0., 1., 0.], [0., 0., 0., 1.]],
dtype=np.float64)
pbrv = MeshcatRigidBodyVisualizer(rbt)
return rbt, pbrv
def runPendulumExample(args):
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder)
parser = Parser(plant)
parser.AddModelFromFile(FindResource("pendulum/pendulum.urdf"))
plant.Finalize()
pose_bundle_output_port = scene_graph.get_pose_bundle_output_port()
Tview = np.array([[1., 0., 0., 0.], [0., 0., 1., 0.], [0., 0., 0., 1.]],
dtype=np.float64)
visualizer = builder.AddSystem(
PlanarSceneGraphVisualizer(scene_graph,
Tview=Tview,
xlim=[-1.2, 1.2],
ylim=[-1.2, 1.2]))
builder.Connect(pose_bundle_output_port, visualizer.get_input_port(0))
diagram = builder.Build()
simulator = Simulator(diagram)
simulator.Initialize()
simulator.set_target_realtime_rate(1.0)
# Fix the input port to zero.
plant_context = diagram.GetMutableSubsystemContext(
plant, simulator.get_mutable_context())
plant_context.FixInputPort(plant.get_actuation_input_port().get_index(),
np.zeros(plant.num_actuators()))
plant_context.SetContinuousState([0.5, 0.1])
simulator.StepTo(args.duration)
def setupDoublePendulumExample():
rbt = RigidBodyTree(FindResource("double_pendulum/double_pendulum.urdf"),
floating_base_type=FloatingBaseType.kFixed) # noqa
Tview = np.array([[1., 0., 0., 0.], [0., 0., 1., 0.], [0., 0., 0., 1.]],
dtype=np.float64)
pbrv = MeshcatRigidBodyVisualizer(rbt)
return rbt, pbrv
def setupDoublePendulumExample():
rbt = RigidBodyTree(FindResource("double_pendulum/double_pendulum.urdf"),
floating_base_type=FloatingBaseType.kFixed) # noqa
Tview = np.array([[1., 0., 0., 0.], [0., 0., 1., 0.], [0., 0., 0., 1.]],
dtype=np.float64)
fig, ax = plt.subplots(1, 1)
pbrv = PlanarRigidBodyVisualizer(rbt, Tview, [-2.5, 2.5], [-2.5, 2.5],
use_random_colors=True, ax=ax)
return rbt, pbrv