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
import pydrake
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 = PlanarRigidBodyVisualizer(rbt, Tview, [-2.0, 2.0], [-0.25, 3.0],
use_random_colors=True)
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 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 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 = PlanarRigidBodyVisualizer(rbt, Tview, [-2.5, 2.5], [-2.5, 2.5],
use_random_colors=True)
return rbt, pbrv
default=1.0)
args = parser.parse_args()
builder = DiagramBuilder()
plant = builder.AddSystem(MultibodyPlant())
scene_graph = builder.AddSystem(SceneGraph())
plant.RegisterAsSourceForSceneGraph(scene_graph)
builder.Connect(plant.get_geometry_poses_output_port(),
scene_graph.get_source_pose_port(
plant.get_source_id()))
builder.Connect(scene_graph.get_query_output_port(),
plant.get_geometry_query_input_port())
parser = Parser(plant)
parser.AddModelFromFile(FindResource("pendulum/pendulum.urdf"))
plant.AddForceElement(UniformGravityFieldElement())
plant.Finalize()
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(scene_graph.get_pose_bundle_output_port(),
visualizer.get_input_port(0))
diagram = builder.Build()
simulator = Simulator(diagram)
simulator.Initialize()
