def test_geometry(self):
builder = DiagramBuilder()
quadrotor = builder.AddSystem(QuadrotorPlant())
scene_graph = builder.AddSystem(SceneGraph())
state_port = quadrotor.get_output_port(0)
geom = QuadrotorGeometry.AddToBuilder(builder, state_port, scene_graph)
self.assertTrue(geom.get_frame_id().is_valid())
def test_kuka(self):
"""Kuka IIWA with mesh geometry."""
file_name = FindResourceOrThrow(
"drake/manipulation/models/iiwa_description/sdf/"
"iiwa14_no_collision.sdf")
builder = DiagramBuilder()
scene_graph = builder.AddSystem(SceneGraph())
kuka = builder.AddSystem(MultibodyPlant())
parser = Parser(plant=kuka, scene_graph=scene_graph)
parser.AddModelFromFile(file_name)
kuka.AddForceElement(UniformGravityFieldElement([0, 0, -9.81]))
kuka.Finalize(scene_graph)
assert kuka.geometry_source_is_registered()
builder.Connect(kuka.get_geometry_poses_output_port(),
scene_graph.get_source_pose_port(kuka.get_source_id()))
visualizer = builder.AddSystem(
MeshcatVisualizer(scene_graph, zmq_url=None, open_browser=False))
builder.Connect(scene_graph.get_pose_bundle_output_port(),
visualizer.get_input_port(0))
diagram = builder.Build()
diagram_context = diagram.CreateDefaultContext()
kuka_context = diagram.GetMutableSubsystemContext(
kuka, diagram_context)
kuka_context.FixInputPort(
kuka.get_actuation_input_port().get_index(),
np.zeros(kuka.get_actuation_input_port().size()))
simulator = Simulator(diagram, diagram_context)
simulator.set_publish_every_time_step(False)
simulator.StepTo(.1)
def RunSimulation(self, real_time_rate=1.0):
'''
The Princess Diaries was a good movie.
'''
builder = DiagramBuilder()
scene_graph = builder.AddSystem(SceneGraph())
# object_file_path = FindResourceOrThrow(
# "drake/examples/manipulation_station/models/061_foam_brick.sdf")
# sdf_file = FindResourceOrThrow("drake/multibody/benchmarks/acrobot/acrobot.sdf")
# urdf_file = FindResourceOrThrow("drake/multibody/benchmarks/acrobot/acrobot.urdf")
sdf_file = "assets/acrobot.sdf"
urdf_file = "assets/acrobot.urdf"
plant = builder.AddSystem(MultibodyPlant())
plant.RegisterAsSourceForSceneGraph(scene_graph)
Parser(plant, scene_graph).AddModelFromFile(sdf_file)
plant.Finalize(scene_graph)
assert plant.geometry_source_is_registered()
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())
# Add
nn_system = NNSystem(self.pytorch_nn_object)
builder.AddSystem(nn_system)
# NN -> plant
builder.Connect(nn_system.NN_out_output_port,
plant.get_actuation_input_port())
# plant -> NN
builder.Connect(plant.get_continuous_state_output_port(),
nn_system.NN_in_input_port)
# Add meshcat visualizer
meshcat = MeshcatVisualizer(scene_graph)
builder.AddSystem(meshcat)
# builder.Connect(scene_graph.GetOutputPort("lcm_visualization"),
builder.Connect(scene_graph.get_pose_bundle_output_port(),
meshcat.GetInputPort("lcm_visualization"))
# build diagram
diagram = builder.Build()
meshcat.load()
# time.sleep(2.0)
RenderSystemWithGraphviz(diagram)
# construct simulator
simulator = Simulator(diagram)
# context = diagram.GetMutableSubsystemContext(
# self.station, simulator.get_mutable_context())
simulator.set_publish_every_time_step(False)
simulator.set_target_realtime_rate(real_time_rate)
simulator.Initialize()
sim_duration = 5.
simulator.StepTo(sim_duration)
print("stepping complete")
def test_custom_geometry_name_parsing(self):
"""
Ensure that name parsing does not fail on programmatically added
anchored geometries.
"""
# Make a minimal example to ensure MeshcatVisualizer loads anchored
# geometry.
builder = DiagramBuilder()
scene_graph = builder.AddSystem(SceneGraph())
meshcat = builder.AddSystem(
MeshcatVisualizer(scene_graph,
zmq_url=ZMQ_URL,
open_browser=False))
builder.Connect(
scene_graph.get_pose_bundle_output_port(),
meshcat.get_input_port(0))
source_id = scene_graph.RegisterSource()
geom_inst = GeometryInstance(RigidTransform(), Box(1., 1., 1.), "box")
geom_id = scene_graph.RegisterAnchoredGeometry(source_id, geom_inst)
# Illustration properties required to ensure geometry is parsed
scene_graph.AssignRole(source_id, geom_id, IllustrationProperties())
diagram = builder.Build()
simulator = Simulator(diagram)
simulator.Initialize()
def __init__(self, config):
self._config = config
builder = DiagramBuilder()
dt = config['env']['mbp_dt']
mbp, sg = AddMultibodyPlantSceneGraph(builder, MultibodyPlant(dt),
SceneGraph())
parser = Parser(mbp, sg)
# renderer_params = RenderEngineVtkParams()
# renderer_name = "vtk_renderer"
# sg.AddRenderer(renderer_name, MakeRenderEngineVtk(renderer_params))
self._mbp = mbp
self._sg = sg
self._parser = parser
self._builder = builder
self._rgbd_sensors = dict()
self._finalized = False
self._built = False
self._diagram = None
# add rendered
# Add renderer to scene
renderer_params = RenderEngineVtkParams()
self._renderer_name = "vtk_renderer"
sg.AddRenderer(self._renderer_name,
MakeRenderEngineVtk(renderer_params))
def main():
parser = argparse.ArgumentParser(description=__doc__)
parser.add_argument(
"--target_realtime_rate",
type=float,
default=1.0,
help="Desired rate relative to real time. See documentation for "
"Simulator::set_target_realtime_rate() for details.")
parser.add_argument("--simulation_time",
type=float,
default=10.0,
help="Desired duration of the simulation in seconds.")
parser.add_argument(
"--time_step",
type=float,
default=0.,
help="If greater than zero, the plant is modeled as a system with "
"discrete updates and period equal to this time_step. "
"If 0, the plant is modeled as a continuous system.")
args = parser.parse_args()
file_name = FindResourceOrThrow(
"drake/examples/multibody/cart_pole/cart_pole.sdf")
builder = DiagramBuilder()
scene_graph = builder.AddSystem(SceneGraph())
cart_pole = builder.AddSystem(MultibodyPlant(time_step=args.time_step))
AddModelFromSdfFile(file_name=file_name,
plant=cart_pole,
scene_graph=scene_graph)
cart_pole.AddForceElement(UniformGravityFieldElement([0, 0, -9.81]))
cart_pole.Finalize(scene_graph)
assert cart_pole.geometry_source_is_registered()
builder.Connect(
cart_pole.get_geometry_poses_output_port(),
scene_graph.get_source_pose_port(cart_pole.get_source_id()))
lcm = DrakeLcm()
ConnectVisualization(scene_graph=scene_graph, builder=builder, lcm=lcm)
diagram = builder.Build()
DispatchLoadMessage(scene_graph=scene_graph, lcm=lcm)
diagram_context = diagram.CreateDefaultContext()
cart_pole_context = diagram.GetMutableSubsystemContext(
cart_pole, diagram_context)
cart_pole_context.FixInputPort(
cart_pole.get_actuation_input_port().get_index(), [0])
cart_slider = cart_pole.GetJointByName("CartSlider")
pole_pin = cart_pole.GetJointByName("PolePin")
cart_slider.set_translation(context=cart_pole_context, translation=0.)
pole_pin.set_angle(context=cart_pole_context, angle=2.)
simulator = Simulator(diagram, diagram_context)
simulator.set_publish_every_time_step(False)
simulator.set_target_realtime_rate(args.target_realtime_rate)
simulator.Initialize()
simulator.StepTo(args.simulation_time)
def test_geometry_with_params(self):
builder = DiagramBuilder()
plant = builder.AddSystem(AcrobotPlant())
scene_graph = builder.AddSystem(SceneGraph())
geom = AcrobotGeometry.AddToBuilder(
builder=builder, acrobot_state_port=plant.get_output_port(0),
acrobot_params=AcrobotParams(), scene_graph=scene_graph)
builder.Build()
self.assertIsInstance(geom, AcrobotGeometry)
def test_geometry(self):
builder = DiagramBuilder()
plant = builder.AddSystem(PendulumPlant())
scene_graph = builder.AddSystem(SceneGraph())
geom = PendulumGeometry.AddToBuilder(
builder=builder, pendulum_state_port=plant.get_output_port(0),
scene_graph=scene_graph)
builder.Build()
self.assertIsInstance(geom, PendulumGeometry)
def main():
parser = argparse.ArgumentParser(description=__doc__)
parser.add_argument(
"--target_realtime_rate",
type=float,
default=1.0,
help="Desired rate relative to real time. See documentation for "
"Simulator::set_target_realtime_rate() for details.")
parser.add_argument("--simulation_time",
type=float,
default=10.0,
help="Desired duration of the simulation in seconds.")
parser.add_argument(
"--time_step",
type=float,
default=0.,
help="If greater than zero, the plant is modeled as a system with "
"discrete updates and period equal to this time_step. "
"If 0, the plant is modeled as a continuous system.")
args = parser.parse_args()
file_name = FindResourceOrThrow(
"drake/examples/multibody/cart_pole/cart_pole.sdf")
builder = DiagramBuilder()
scene_graph = builder.AddSystem(SceneGraph())
cart_pole = builder.AddSystem(MultibodyPlant(time_step=args.time_step))
cart_pole.RegisterAsSourceForSceneGraph(scene_graph)
Parser(plant=cart_pole).AddModelFromFile(file_name)
cart_pole.Finalize()
assert cart_pole.geometry_source_is_registered()
builder.Connect(scene_graph.get_query_output_port(),
cart_pole.get_geometry_query_input_port())
builder.Connect(
cart_pole.get_geometry_poses_output_port(),
scene_graph.get_source_pose_port(cart_pole.get_source_id()))
DrakeVisualizer.AddToBuilder(builder=builder, scene_graph=scene_graph)
diagram = builder.Build()
diagram_context = diagram.CreateDefaultContext()
cart_pole_context = diagram.GetMutableSubsystemContext(
cart_pole, diagram_context)
cart_pole.get_actuation_input_port().FixValue(cart_pole_context, 0)
cart_slider = cart_pole.GetJointByName("CartSlider")
pole_pin = cart_pole.GetJointByName("PolePin")
cart_slider.set_translation(context=cart_pole_context, translation=0.)
pole_pin.set_angle(context=cart_pole_context, angle=2.)
simulator = Simulator(diagram, diagram_context)
simulator.set_publish_every_time_step(False)
simulator.set_target_realtime_rate(args.target_realtime_rate)
simulator.Initialize()
simulator.AdvanceTo(args.simulation_time)
def add_plant_and_scene_graph(builder):
# TODO(eric.cousineau): Hoist to C++.
plant = builder.AddSystem(MultibodyPlant())
scene_graph = builder.AddSystem(SceneGraph())
plant.RegisterAsSourceForSceneGraph(scene_graph)
builder.Connect(scene_graph.get_query_output_port(),
plant.get_geometry_query_input_port())
builder.Connect(plant.get_geometry_poses_output_port(),
scene_graph.get_source_pose_port(plant.get_source_id()))
return plant, scene_graph
def build_block_diagram(actuators_off=False,
desired_lateral_velocity=0.0,
desired_height=3.0,
print_period=0.0):
builder = DiagramBuilder()
# Build the plant
plant = builder.AddSystem(MultibodyPlant(0.0005))
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())
# Build the robot
parser = Parser(plant)
parser.AddModelFromFile("raibert_hopper_2d.sdf")
plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("ground"))
plant.Finalize()
plant.set_name('plant')
# Create a logger to log at 30hz
state_dim = plant.num_positions() + plant.num_velocities()
state_log = builder.AddSystem(SignalLogger(state_dim))
state_log.DeclarePeriodicPublish(0.0333, 0.0) # 30hz logging
builder.Connect(plant.get_state_output_port(), state_log.get_input_port(0))
state_log.set_name('state_log')
# The controller
controller = builder.AddSystem(
Hopper2dController(plant,
desired_lateral_velocity=desired_lateral_velocity,
desired_height=desired_height,
actuators_off=actuators_off,
print_period=print_period))
builder.Connect(plant.get_state_output_port(),
controller.get_input_port(0))
builder.Connect(controller.get_output_port(0),
plant.get_actuation_input_port())
controller.set_name('controller')
# Create visualizer
visualizer = builder.AddSystem(
PlanarSceneGraphVisualizer(scene_graph,
xlim=[-1, 10],
ylim=[-.2, 4.5],
show=False))
builder.Connect(scene_graph.get_pose_bundle_output_port(),
visualizer.get_input_port(0))
visualizer.set_name('visualizer')
diagram = builder.Build()
return diagram
def test_geometry_with_params(self):
builder = DiagramBuilder()
plant = builder.AddSystem(RimlessWheel())
scene_graph = builder.AddSystem(SceneGraph())
geom = RimlessWheelGeometry.AddToBuilder(
builder=builder,
floating_base_state_port=plant.get_floating_base_state_output_port(), # noqa
rimless_wheel_params=RimlessWheelParams(), scene_graph=scene_graph)
# Confirming that the resulting diagram builds.
builder.Build()
self.assertIsInstance(geom, RimlessWheelGeometry)
def test_geometry(self):
builder = DiagramBuilder()
plant = builder.AddSystem(CompassGait())
scene_graph = builder.AddSystem(SceneGraph())
geom = CompassGaitGeometry.AddToBuilder(
builder=builder,
floating_base_state_port=plant.get_floating_base_state_output_port(
), # noqa
scene_graph=scene_graph)
# Confirming that the resulting diagram builds.
builder.Build()
self.assertIsInstance(geom, CompassGaitGeometry)
def load_tables(time_step=0.0):
mbp = MultibodyPlant(time_step=time_step)
scene_graph = SceneGraph()
# TODO: meshes aren't supported during collision checking
robot = AddModelFromSdfFile(file_name=IIWA14_SDF_PATH, model_name='iiwa',
scene_graph=scene_graph, plant=mbp)
gripper = AddModelFromSdfFile(file_name=WSG50_SDF_PATH, model_name='gripper',
scene_graph=scene_graph, plant=mbp) # TODO: sdf frame/link error
table = AddModelFromSdfFile(file_name=TABLE_SDF_PATH, model_name='table',
scene_graph=scene_graph, plant=mbp)
table2 = AddModelFromSdfFile(file_name=TABLE_SDF_PATH, model_name='table2',
scene_graph=scene_graph, plant=mbp)
sink = AddModelFromSdfFile(file_name=SINK_PATH, model_name='sink',
scene_graph=scene_graph, plant=mbp)
stove = AddModelFromSdfFile(file_name=STOVE_PATH, model_name='stove',
scene_graph=scene_graph, plant=mbp)
broccoli = AddModelFromSdfFile(file_name=BROCCOLI_PATH, model_name='broccoli',
scene_graph=scene_graph, plant=mbp)
#wall = AddModelFromSdfFile(file_name=WALL_PATH, model_name='wall',
# scene_graph=scene_graph, plant=mbp)
wall = None
table2_x = 0.75
table_top_z = get_aabb_z_placement(AABBs['sink'], AABBs['table']) # TODO: use geometry
weld_gripper(mbp, robot, gripper)
weld_to_world(mbp, robot, create_transform(translation=[0, 0, table_top_z]))
weld_to_world(mbp, table, create_transform())
weld_to_world(mbp, table2, create_transform(translation=[table2_x, 0, 0]))
weld_to_world(mbp, sink, create_transform(translation=[table2_x, 0.25, table_top_z]))
weld_to_world(mbp, stove, create_transform(translation=[table2_x, -0.25, table_top_z]))
if wall is not None:
weld_to_world(mbp, wall, create_transform(translation=[table2_x / 2, 0, table_top_z]))
mbp.Finalize(scene_graph)
movable = [broccoli]
surfaces = [
VisualElement(sink, 'base_link', 0), # Could also just pass the link index
VisualElement(stove, 'base_link', 0),
]
initial_poses = {
broccoli: create_transform(translation=[table2_x, 0, table_top_z]),
}
task = Task(mbp, scene_graph, robot, gripper,
movable=movable, surfaces=surfaces,
initial_poses=initial_poses,
goal_cooked=[broccoli])
return mbp, scene_graph, task
def Simulate2dHopper(x0,
duration,
desired_lateral_velocity=0.0,
print_period=0.0):
builder = DiagramBuilder()
plant = builder.AddSystem(MultibodyPlant(0.0005))
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())
# Build the plant
parser = Parser(plant)
parser.AddModelFromFile("raibert_hopper_2d.sdf")
plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("ground"))
plant.AddForceElement(UniformGravityFieldElement())
plant.Finalize()
# Create a logger to log at 30hz
state_dim = plant.num_positions() + plant.num_velocities()
state_log = builder.AddSystem(SignalLogger(state_dim))
state_log.DeclarePeriodicPublish(0.0333, 0.0) # 30hz logging
builder.Connect(plant.get_continuous_state_output_port(),
state_log.get_input_port(0))
# The controller
controller = builder.AddSystem(
Hopper2dController(plant,
desired_lateral_velocity=desired_lateral_velocity,
print_period=print_period))
builder.Connect(plant.get_continuous_state_output_port(),
controller.get_input_port(0))
builder.Connect(controller.get_output_port(0),
plant.get_actuation_input_port())
# The diagram
diagram = builder.Build()
simulator = Simulator(diagram)
simulator.Initialize()
plant_context = diagram.GetMutableSubsystemContext(
plant, simulator.get_mutable_context())
plant_context.get_mutable_discrete_state_vector().SetFromVector(x0)
simulator.StepTo(duration)
return plant, controller, state_log
def main():
parser = argparse.ArgumentParser(
description=__doc__,
formatter_class=argparse.RawDescriptionHelpFormatter)
parser.add_argument(
"filename", type=str,
help="Path to an SDF or URDF file.")
MeshcatVisualizer.add_argparse_argument(parser)
args = parser.parse_args()
filename = args.filename
if not os.path.isfile(filename):
parser.error("File does not exist: {}".format(filename))
# Construct Plant + SceneGraph.
builder = DiagramBuilder()
plant = builder.AddSystem(MultibodyPlant())
scene_graph = builder.AddSystem(SceneGraph())
plant.RegisterAsSourceForSceneGraph(scene_graph)
builder.Connect(
scene_graph.get_query_output_port(),
plant.get_geometry_query_input_port())
builder.Connect(
plant.get_geometry_poses_output_port(),
scene_graph.get_source_pose_port(plant.get_source_id()))
# Load the model file.
Parser(plant).AddModelFromFile(filename)
plant.Finalize()
# Publish to Drake Visualizer.
drake_viz_pub = ConnectDrakeVisualizer(builder, scene_graph)
# Publish to Meshcat.
if args.meshcat is not None:
meshcat_viz = builder.AddSystem(
MeshcatVisualizer(scene_graph, zmq_url=args.meshcat))
builder.Connect(
scene_graph.get_pose_bundle_output_port(),
meshcat_viz.get_input_port(0))
diagram = builder.Build()
context = diagram.CreateDefaultContext()
# Use Simulator to dispatch initialization events.
# TODO(eric.cousineau): Simplify as part of #10015.
Simulator(diagram).Initialize()
# Publish draw messages with current state.
diagram.Publish(context)
def testMultibodyPositionToGeometryPose(self):
file_name = FindResourceOrThrow(
"drake/multibody/benchmarks/acrobot/acrobot.sdf")
plant = MultibodyPlant(time_step=0.01)
model_instance = AddModelFromSdfFile(file_name=file_name, plant=plant)
scene_graph = SceneGraph()
plant.RegisterAsSourceForSceneGraph(scene_graph)
plant.Finalize()
to_pose = MultibodyPositionToGeometryPose(plant)
# Check the size of the input.
self.assertEqual(to_pose.get_input_port().size(), 2)
# Just check the spelling of the output port (size is not meaningful
# for Abstract-valued ports).
to_pose.get_output_port()
def test_basics(self):
# call default constructor
QuadrotorPlant()
quadrotor = QuadrotorPlant(m_arg=1,
L_arg=2,
I_arg=np.eye(3),
kF_arg=1.,
kM_arg=1.)
self.assertEqual(quadrotor.m(), 1)
self.assertEqual(quadrotor.g(), 9.81)
scene_graph = SceneGraph()
quadrotor.RegisterGeometry(scene_graph)
self.assertTrue(quadrotor.source_id().is_valid())
quadrotor.get_geometry_pose_output_port()
StabilizingLQRController(quadrotor, np.zeros(3))
def cartPoleTest(self):
file_name = FindResourceOrThrow(
"drake/examples/multibody/cart_pole/cart_pole.sdf")
builder = DiagramBuilder()
scene_graph = builder.AddSystem(SceneGraph())
cart_pole = builder.AddSystem(MultibodyPlant())
AddModelFromSdfFile(file_name=file_name,
plant=cart_pole,
scene_graph=scene_graph)
cart_pole.AddForceElement(UniformGravityFieldElement([0, 0, -9.81]))
cart_pole.Finalize(scene_graph)
assert cart_pole.geometry_source_is_registered()
builder.Connect(
cart_pole.get_geometry_poses_output_port(),
scene_graph.get_source_pose_port(cart_pole.get_source_id()))
visualizer = builder.AddSystem(
MeshcatVisualizer(scene_graph, zmq_url=None))
builder.Connect(scene_graph.get_pose_bundle_output_port(),
visualizer.get_input_port(0))
diagram = builder.Build()
visualizer.load()
diagram_context = diagram.CreateDefaultContext()
cart_pole_context = diagram.GetMutableSubsystemContext(
cart_pole, diagram_context)
cart_pole_context.FixInputPort(
cart_pole.get_actuation_input_port().get_index(), [0])
cart_slider = cart_pole.GetJointByName("CartSlider")
pole_pin = cart_pole.GetJointByName("PolePin")
cart_slider.set_translation(context=cart_pole_context, translation=0.)
pole_pin.set_angle(context=cart_pole_context, angle=2.)
simulator = Simulator(diagram, diagram_context)
simulator.set_publish_every_time_step(False)
simulator.set_target_realtime_rate(args.target_realtime_rate)
simulator.Initialize()
simulator.StepTo(args.duration)
def kukaTest(args):
file_name = FindResourceOrThrow(
"drake/manipulation/models/iiwa_description/sdf/iiwa14_no_collision"
".sdf")
builder = DiagramBuilder()
scene_graph = builder.AddSystem(SceneGraph())
kuka = builder.AddSystem(MultibodyPlant())
AddModelFromSdfFile(
file_name=file_name, plant=kuka, scene_graph=scene_graph)
kuka.AddForceElement(UniformGravityFieldElement([0, 0, -9.81]))
kuka.Finalize(scene_graph)
assert kuka.geometry_source_is_registered()
builder.Connect(
kuka.get_geometry_poses_output_port(),
scene_graph.get_source_pose_port(kuka.get_source_id()))
visualizer = builder.AddSystem(MeshcatVisualizer(scene_graph))
builder.Connect(scene_graph.get_pose_bundle_output_port(),
visualizer.get_input_port(0))
diagram = builder.Build()
visualizer.load()
diagram_context = diagram.CreateDefaultContext()
kuka_context = diagram.GetMutableSubsystemContext(
kuka, diagram_context)
kuka_context.FixInputPort(
kuka.get_actuation_input_port().get_index(), np.zeros(
kuka.get_actuation_input_port().size()))
simulator = Simulator(diagram, diagram_context)
simulator.set_publish_every_time_step(False)
simulator.set_target_realtime_rate(args.target_realtime_rate)
simulator.Initialize()
simulator.StepTo(args.duration)
def load_manipulation(time_step=0.0,
use_meshcat=True,
use_controllers=True,
use_external=False):
if not use_external:
AMAZON_TABLE_PATH = FindResourceOrThrow(
"drake/examples/manipulation_station/models/amazon_table_simplified.sdf"
)
CUPBOARD_PATH = FindResourceOrThrow(
"drake/examples/manipulation_station/models/cupboard.sdf")
IIWA7_PATH = FindResourceOrThrow(
"drake/manipulation/models/iiwa_description/iiwa7/iiwa7_with_box_collision.sdf"
)
FOAM_BRICK_PATH = FindResourceOrThrow(
"drake/examples/manipulation_station/models/061_foam_brick.sdf")
goal_shelf = 'shelf_lower'
#goal_shelf = 'shelf_upper'
else:
AMAZON_TABLE_PATH = FindResourceOrThrow(
"drake/external/models_robotlocomotion/manipulation_station/amazon_table_simplified.sdf"
)
CUPBOARD_PATH = FindResourceOrThrow(
"drake/external/models_robotlocomotion/manipulation_station/cupboard.sdf"
)
IIWA7_PATH = FindResourceOrThrow(
"drake/external/models_robotlocomotion/iiwa7/iiwa7_no_collision.sdf"
)
FOAM_BRICK_PATH = FindResourceOrThrow(
"drake/external/models_robotlocomotion/ycb_objects/061_foam_brick.sdf"
)
goal_shelf = 'bottom'
#IIWA7_PATH = os.path.join(MODELS_DIR, "iiwa_description/iiwa7/iiwa7_with_box_collision.sdf")
plant = MultibodyPlant(time_step=time_step)
scene_graph = SceneGraph()
dx_table_center_to_robot_base = 0.3257
dz_table_top_robot_base = 0.0127
dx_cupboard_to_table_center = 0.43 + 0.15
dz_cupboard_to_table_center = 0.02
cupboard_height = 0.815
cupboard_x = dx_table_center_to_robot_base + dx_cupboard_to_table_center
cupboard_z = dz_cupboard_to_table_center + cupboard_height / 2.0 - dz_table_top_robot_base
robot = AddModelFromSdfFile(file_name=IIWA7_PATH,
model_name='iiwa',
scene_graph=scene_graph,
plant=plant)
gripper = AddModelFromSdfFile(file_name=WSG50_SDF_PATH,
model_name='gripper',
scene_graph=scene_graph,
plant=plant) # TODO: sdf frame/link error
table = AddModelFromSdfFile(file_name=AMAZON_TABLE_PATH,
model_name='table',
scene_graph=scene_graph,
plant=plant)
cupboard = AddModelFromSdfFile(file_name=CUPBOARD_PATH,
model_name='cupboard',
scene_graph=scene_graph,
plant=plant)
brick = AddModelFromSdfFile(file_name=FOAM_BRICK_PATH,
model_name='brick',
scene_graph=scene_graph,
plant=plant)
weld_gripper(plant, robot, gripper)
weld_to_world(plant, robot, create_transform())
weld_to_world(
plant, table,
create_transform(translation=[
dx_table_center_to_robot_base, 0, -dz_table_top_robot_base
]))
weld_to_world(
plant, cupboard,
create_transform(translation=[cupboard_x, 0, cupboard_z],
rotation=[0, 0, np.pi]))
plant.Finalize(scene_graph)
shelves = [
'bottom',
'shelf_lower',
'shelf_upper',
'top',
]
goal_surface = Surface(plant, cupboard, 'top_and_bottom',
shelves.index(goal_shelf))
surfaces = [
#Surface(plant, table, 'amazon_table', 0),
#goal_surface,
]
door_names = ['left_door', 'right_door']
#door_names = []
doors = [plant.GetBodyByName(name).index() for name in door_names]
door_position = DOOR_CLOSED # ~np.pi/2
#door_position = DOOR_OPEN
#door_position = np.pi
#door_position = np.pi/2
#door_position = np.pi/8
initial_positions = {
plant.GetJointByName('left_door_hinge'):
-door_position,
#plant.GetJointByName('right_door_hinge'): door_position,
plant.GetJointByName('right_door_hinge'):
np.pi / 2,
}
initial_conf = [0, 0.6 - np.pi / 6, 0, -1.75, 0, 1.0, 0]
#initial_conf[1] += np.pi / 6
initial_positions.update(
zip(get_movable_joints(plant, robot), initial_conf))
initial_poses = {
#brick: create_transform(translation=[0.6, 0, 0]),
brick:
create_transform(translation=[0.3, 0, 0], rotation=[0, 0, np.pi / 2]),
#brick: create_transform(translation=[0.2, -0.3, 0], rotation=[0, 0, np.pi/8]),
}
goal_poses = {
brick:
create_transform(translation=[0.8, 0.2, 0.2927],
rotation=[0, 0, np.pi / 8]),
}
goal_holding = [
#brick,
]
goal_on = [
#(brick, goal_surface),
]
diagram, state_machine = build_diagram(plant,
scene_graph,
robot,
gripper,
meshcat=use_meshcat,
controllers=use_controllers)
task = Task(diagram,
plant,
scene_graph,
robot,
gripper,
movable=[brick],
surfaces=surfaces,
doors=doors,
initial_positions=initial_positions,
initial_poses=initial_poses,
goal_poses=goal_poses,
goal_holding=goal_holding,
goal_on=goal_on,
reset_robot=True,
reset_doors=False)
return task, diagram, state_machine
def main():
args_parser = argparse.ArgumentParser(
description=__doc__,
formatter_class=argparse.RawDescriptionHelpFormatter)
add_filename_and_parser_argparse_arguments(args_parser)
add_visualizers_argparse_arguments(args_parser)
args_parser.add_argument(
"--position", type=float, nargs="+", default=[],
help="A list of positions which must be the same length as the number "
"of positions in the sdf model. Note that most models have a "
"floating-base joint by default (unless the sdf explicitly welds "
"the base to the world, and so have 7 positions corresponding to "
"the quaternion representation of that floating-base position.")
# TODO(eric.cousineau): Support sliders (or widgets) for floating body
# poses.
# TODO(russt): Once floating body sliders are supported, add an option to
# disable them too, either by welding via GetUniqueBaseBody #9747 or by
# hiding the widgets.
args_parser.add_argument(
"--test", action='store_true',
help="Disable opening the slider gui window for testing.")
args = args_parser.parse_args()
# NOTE: meshcat is required to create the JointSliders.
args.meshcat = True
filename, make_parser = parse_filename_and_parser(args_parser, args)
update_visualization, connect_visualizers = parse_visualizers(
args_parser, args)
builder = DiagramBuilder()
scene_graph = builder.AddSystem(SceneGraph())
# Construct a MultibodyPlant.
# N.B. Do not use AddMultibodyPlantSceneGraph because we want to inject our
# custom pose-bundle adjustments for the sliders.
plant = MultibodyPlant(time_step=0.0)
plant.RegisterAsSourceForSceneGraph(scene_graph)
# Add the model from the file and finalize the plant.
make_parser(plant).AddModelFromFile(filename)
update_visualization(plant, scene_graph)
plant.Finalize()
meshcat = connect_visualizers(builder, plant, scene_graph)
assert meshcat is not None, "Meshcat visualizer not created but required."
# Add sliders to set positions of the joints.
sliders = builder.AddSystem(JointSliders(meshcat=meshcat, plant=plant))
to_pose = builder.AddSystem(MultibodyPositionToGeometryPose(plant))
builder.Connect(sliders.get_output_port(0), to_pose.get_input_port())
builder.Connect(
to_pose.get_output_port(),
scene_graph.get_source_pose_port(plant.get_source_id()))
if len(args.position):
sliders.SetPositions(args.position)
# Make the diagram and run it.
diagram = builder.Build()
simulator = Simulator(diagram)
if args.test:
simulator.AdvanceTo(0.1)
else:
simulator.set_target_realtime_rate(1.0)
simulator.AdvanceTo(np.inf)
def main():
args_parser = argparse.ArgumentParser(
description=__doc__,
formatter_class=argparse.RawDescriptionHelpFormatter)
args_parser.add_argument(
"filename", type=str,
help="Path to an SDF or URDF file.")
args_parser.add_argument(
"--package_path",
type=str,
default=None,
help="Full path to the root package for reading in SDF resources.")
position_group = args_parser.add_mutually_exclusive_group()
position_group.add_argument(
"--position", type=float, nargs="+", default=[],
help="A list of positions which must be the same length as the number "
"of positions in the sdf model. Note that most models have a "
"floating-base joint by default (unless the sdf explicitly welds "
"the base to the world, and so have 7 positions corresponding to "
"the quaternion representation of that floating-base position.")
position_group.add_argument(
"--joint_position", type=float, nargs="+", default=[],
help="A list of positions which must be the same length as the number "
"of positions ASSOCIATED WITH JOINTS in the sdf model. This "
"does not include, e.g., floating-base coordinates, which will "
"be assigned a default value.")
args_parser.add_argument(
"--test", action='store_true',
help="Disable opening the gui window for testing.")
# TODO(russt): Add option to weld the base to the world pending the
# availability of GetUniqueBaseBody requested in #9747.
MeshcatVisualizer.add_argparse_argument(args_parser)
args = args_parser.parse_args()
filename = args.filename
if not os.path.isfile(filename):
args_parser.error("File does not exist: {}".format(filename))
builder = DiagramBuilder()
scene_graph = builder.AddSystem(SceneGraph())
# Construct a MultibodyPlant.
plant = MultibodyPlant(0.0)
plant.RegisterAsSourceForSceneGraph(scene_graph)
# Create the parser.
parser = Parser(plant)
# Get the package pathname.
if args.package_path:
# Verify that package.xml is found in the designated path.
package_path = os.path.abspath(args.package_path)
if not os.path.isfile(os.path.join(package_path, "package.xml")):
parser.error("package.xml not found at: {}".format(package_path))
# Get the package map and populate it using the package path.
package_map = parser.package_map()
package_map.PopulateFromFolder(package_path)
# Add the model from the file and finalize the plant.
parser.AddModelFromFile(filename)
plant.Finalize()
# Add sliders to set positions of the joints.
sliders = builder.AddSystem(JointSliders(robot=plant))
to_pose = builder.AddSystem(MultibodyPositionToGeometryPose(plant))
builder.Connect(sliders.get_output_port(0), to_pose.get_input_port())
builder.Connect(
to_pose.get_output_port(),
scene_graph.get_source_pose_port(plant.get_source_id()))
# Connect this to drake_visualizer.
ConnectDrakeVisualizer(builder=builder, scene_graph=scene_graph)
# Connect to Meshcat.
if args.meshcat is not None:
meshcat_viz = builder.AddSystem(
MeshcatVisualizer(scene_graph, zmq_url=args.meshcat))
builder.Connect(
scene_graph.get_pose_bundle_output_port(),
meshcat_viz.get_input_port(0))
if len(args.position):
sliders.set_position(args.position)
elif len(args.joint_position):
sliders.set_joint_position(args.joint_position)
# Make the diagram and run it.
diagram = builder.Build()
simulator = Simulator(diagram)
simulator.set_publish_every_time_step(False)
if args.test:
sliders.window.withdraw()
simulator.AdvanceTo(0.1)
else:
simulator.set_target_realtime_rate(1.0)
simulator.AdvanceTo(np.inf)
def test_make_from_scene_graph_and_iris(self):
"""
Tests the make from scene graph and iris functionality together as
the Iris code makes obstacles from geometries registered in SceneGraph.
"""
scene_graph = SceneGraph()
source_id = scene_graph.RegisterSource("source")
frame_id = scene_graph.RegisterFrame(
source_id=source_id, frame=GeometryFrame("frame"))
box_geometry_id = scene_graph.RegisterGeometry(
source_id=source_id, frame_id=frame_id,
geometry=GeometryInstance(X_PG=RigidTransform(),
shape=Box(1., 1., 1.),
name="box"))
cylinder_geometry_id = scene_graph.RegisterGeometry(
source_id=source_id, frame_id=frame_id,
geometry=GeometryInstance(X_PG=RigidTransform(),
shape=Cylinder(1., 1.),
name="cylinder"))
sphere_geometry_id = scene_graph.RegisterGeometry(
source_id=source_id, frame_id=frame_id,
geometry=GeometryInstance(X_PG=RigidTransform(),
shape=Sphere(1.), name="sphere"))
capsule_geometry_id = scene_graph.RegisterGeometry(
source_id=source_id,
frame_id=frame_id,
geometry=GeometryInstance(X_PG=RigidTransform(),
shape=Capsule(1., 1.0),
name="capsule"))
context = scene_graph.CreateDefaultContext()
pose_vector = FramePoseVector()
pose_vector.set_value(frame_id, RigidTransform())
scene_graph.get_source_pose_port(source_id).FixValue(
context, pose_vector)
query_object = scene_graph.get_query_output_port().Eval(context)
H = mut.HPolyhedron(
query_object=query_object, geometry_id=box_geometry_id,
reference_frame=scene_graph.world_frame_id())
self.assertEqual(H.ambient_dimension(), 3)
C = mut.CartesianProduct(
query_object=query_object, geometry_id=cylinder_geometry_id,
reference_frame=scene_graph.world_frame_id())
self.assertEqual(C.ambient_dimension(), 3)
E = mut.Hyperellipsoid(
query_object=query_object, geometry_id=sphere_geometry_id,
reference_frame=scene_graph.world_frame_id())
self.assertEqual(E.ambient_dimension(), 3)
S = mut.MinkowskiSum(
query_object=query_object, geometry_id=capsule_geometry_id,
reference_frame=scene_graph.world_frame_id())
self.assertEqual(S.ambient_dimension(), 3)
P = mut.Point(
query_object=query_object, geometry_id=sphere_geometry_id,
reference_frame=scene_graph.world_frame_id(),
maximum_allowable_radius=1.5)
self.assertEqual(P.ambient_dimension(), 3)
V = mut.VPolytope(
query_object=query_object, geometry_id=box_geometry_id,
reference_frame=scene_graph.world_frame_id())
self.assertEqual(V.ambient_dimension(), 3)
obstacles = mut.MakeIrisObstacles(
query_object=query_object,
reference_frame=scene_graph.world_frame_id())
options = mut.IrisOptions()
options.require_sample_point_is_contained = True
options.iteration_limit = 1
options.termination_threshold = 0.1
options.relative_termination_threshold = 0.01
self.assertNotIn("object at 0x", repr(options))
region = mut.Iris(
obstacles=obstacles, sample=[2, 3.4, 5],
domain=mut.HPolyhedron.MakeBox(
lb=[-5, -5, -5], ub=[5, 5, 5]), options=options)
self.assertIsInstance(region, mut.HPolyhedron)
obstacles = [
mut.HPolyhedron.MakeUnitBox(3),
mut.Hyperellipsoid.MakeUnitBall(3),
mut.Point([0, 0, 0]),
mut.VPolytope.MakeUnitBox(3)]
region = mut.Iris(
obstacles=obstacles, sample=[2, 3.4, 5],
domain=mut.HPolyhedron.MakeBox(
lb=[-5, -5, -5], ub=[5, 5, 5]), options=options)
self.assertIsInstance(region, mut.HPolyhedron)
def LittleDogSimulationDiagram2(lcm, rb_tree, dt, drake_visualizer):
builder = DiagramBuilder()
num_pos = rb_tree.get_num_positions()
num_actuators = rb_tree.get_num_actuators()
zero_config = np.zeros(
(rb_tree.get_num_actuators() * 2, 1)) # just for test
# zero_config = np.concatenate((np.array([0, 1, 1, 1, 1.7, 0.5, 0, 0, 0]),np.zeros(9)), axis=0)
# zero_config = np.concatenate((rb_tree.getZeroConfiguration(),np.zeros(9)), axis=0)
zero_config = np.concatenate(
(np.array([1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]), np.zeros(12)), axis=0)
# zero_config[0] = 1.7
# zero_config[1] = 1.7
# zero_config[2] = 1.7
# zero_config[3] = 1.7
# 1.SceneGraph system
scene_graph = builder.AddSystem(SceneGraph())
scene_graph.RegisterSource('scene_graph_n')
plant = builder.AddSystem(MultibodyPlant())
plant_parser = Parser(plant, scene_graph)
plant_parser.AddModelFromFile(file_name=model_path, model_name="littledog")
plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("body"))
# Add gravity to the model.
plant.AddForceElement(UniformGravityFieldElement([0, 0, -9.81]))
# Model is completed
plant.Finalize()
builder.Connect(scene_graph.get_query_output_port(),
plant.get_geometry_query_input_port())
builder.Connect(plant.get_geometry_poses_output_port(),
scene_graph.get_source_pose_port(plant.get_source_id()))
ConnectDrakeVisualizer(builder=builder, scene_graph=scene_graph)
# Robot State Encoder
robot_state_encoder = builder.AddSystem(
RobotStateEncoder(rb_tree)) # force_sensor_info
robot_state_encoder.set_name('robot_state_encoder')
builder.Connect(plant.get_continuous_state_output_port(),
robot_state_encoder.joint_state_results_input_port())
# Robot command to Plant Converter
robot_command_to_rigidbodyplant_converter = builder.AddSystem(
RobotCommandToRigidBodyPlantConverter(
rb_tree.actuators,
motor_gain=None)) # input argu: rigidbody actuators
robot_command_to_rigidbodyplant_converter.set_name(
'robot_command_to_rigidbodyplant_converter')
builder.Connect(
robot_command_to_rigidbodyplant_converter.desired_effort_output_port(),
plant.get_actuation_input_port())
# PID controller
kp, ki, kd = joints_PID_params(rb_tree)
# PIDcontroller = builder.AddSystem(RobotPDAndFeedForwardController(rb_tree, kp, ki, kd))
# PIDcontroller.set_name('PID_controller')
rb = RigidBodyTree()
robot_frame = RigidBodyFrame("robot_frame", rb_tree.world(), [0, 0, 0],
[0, 0, 0])
# insert a robot from urdf files
pmap = PackageMap()
pmap.PopulateFromFolder(os.path.dirname(model_path))
AddModelInstanceFromUrdfStringSearchingInRosPackages(
open(model_path, 'r').read(),
pmap,
os.path.dirname(model_path),
FloatingBaseType.kFixed, # FloatingBaseType.kRollPitchYaw,
robot_frame,
rb)
PIDcontroller = builder.AddSystem(
RbtInverseDynamicsController(rb, kp, ki, kd, False))
PIDcontroller.set_name('PID_controller')
builder.Connect(robot_state_encoder.joint_state_outport_port(),
PIDcontroller.get_input_port(0))
builder.Connect(
PIDcontroller.get_output_port(0),
robot_command_to_rigidbodyplant_converter.robot_command_input_port())
# Ref trajectory
traj_src = builder.AddSystem(ConstantVectorSource(zero_config))
builder.Connect(traj_src.get_output_port(0),
PIDcontroller.get_input_port(1))
# Signal logger
logger = builder.AddSystem(SignalLogger(num_pos * 2))
builder.Connect(plant.get_continuous_state_output_port(),
logger.get_input_port(0))
return builder.Build(), logger, plant
def load_manipulation(time_step=0.0):
source = True
if source:
AMAZON_TABLE_PATH = FindResourceOrThrow(
"drake/examples/manipulation_station/models/amazon_table_simplified.sdf")
CUPBOARD_PATH = FindResourceOrThrow(
"drake/examples/manipulation_station/models/cupboard.sdf")
#IIWA7_PATH = FindResourceOrThrow(
# "drake/manipulation/models/iiwa_description/iiwa7/iiwa7_with_box_collision.sdf")
IIWA7_PATH = os.path.join(MODELS_DIR, "iiwa_description/iiwa7/iiwa7_with_box_collision.sdf")
FOAM_BRICK_PATH = FindResourceOrThrow(
"drake/examples/manipulation_station/models/061_foam_brick.sdf")
goal_shelf = 'shelf_lower'
else:
AMAZON_TABLE_PATH = FindResourceOrThrow(
"drake/external/models_robotlocomotion/manipulation_station/amazon_table_simplified.sdf")
CUPBOARD_PATH = FindResourceOrThrow(
"drake/external/models_robotlocomotion/manipulation_station/cupboard.sdf")
IIWA7_PATH = FindResourceOrThrow(
"drake/external/models_robotlocomotion/iiwa7/iiwa7_no_collision.sdf")
#IIWA7_PATH = os.path.join(MODELS_DIR, "iiwa_description/iiwa7/iiwa7_with_box_collision.sdf")
FOAM_BRICK_PATH = FindResourceOrThrow(
"drake/external/models_robotlocomotion/ycb_objects/061_foam_brick.sdf")
goal_shelf = 'bottom'
mbp = MultibodyPlant(time_step=time_step)
scene_graph = SceneGraph()
dx_table_center_to_robot_base = 0.3257
dz_table_top_robot_base = 0.0127
dx_cupboard_to_table_center = 0.43 + 0.15
dz_cupboard_to_table_center = 0.02
cupboard_height = 0.815
cupboard_x = dx_table_center_to_robot_base + dx_cupboard_to_table_center
cupboard_z = dz_cupboard_to_table_center + cupboard_height / 2.0 - dz_table_top_robot_base
robot = AddModelFromSdfFile(file_name=IIWA7_PATH, model_name='iiwa',
scene_graph=scene_graph, plant=mbp)
gripper = AddModelFromSdfFile(file_name=WSG50_SDF_PATH, model_name='gripper',
scene_graph=scene_graph, plant=mbp) # TODO: sdf frame/link error
amazon_table = AddModelFromSdfFile(file_name=AMAZON_TABLE_PATH, model_name='amazon_table',
scene_graph=scene_graph, plant=mbp)
cupboard = AddModelFromSdfFile(file_name=CUPBOARD_PATH, model_name='cupboard',
scene_graph=scene_graph, plant=mbp)
brick = AddModelFromSdfFile(file_name=FOAM_BRICK_PATH, model_name='brick',
scene_graph=scene_graph, plant=mbp)
# left_door, left_door_hinge, cylinder
weld_gripper(mbp, robot, gripper)
weld_to_world(mbp, robot, create_transform())
weld_to_world(mbp, amazon_table, create_transform(
translation=[dx_table_center_to_robot_base, 0, -dz_table_top_robot_base]))
weld_to_world(mbp, cupboard, create_transform(
translation=[cupboard_x, 0, cupboard_z], rotation=[0, 0, np.pi]))
mbp.Finalize(scene_graph)
shelves = [
'bottom',
'shelf_lower',
'shelf_upper'
'top',
]
goal_surface = VisualElement(cupboard, 'top_and_bottom', shelves.index(goal_shelf))
surfaces = [
#VisualElement(amazon_table, 'amazon_table', 0),
goal_surface,
]
door_names = [
'left_door',
'right_door',
]
doors = [mbp.GetBodyByName(name).index() for name in door_names]
door_position = DOOR_CLOSED # np.pi/2
#door_position = DOOR_OPEN
#door_position = np.pi
#door_position = np.pi/8
initial_positions = {
mbp.GetJointByName('left_door_hinge'): -door_position,
#mbp.GetJointByName('right_door_hinge'): door_position,
mbp.GetJointByName('right_door_hinge'): np.pi,
}
initial_conf = [0, 0.6 - np.pi / 6, 0, -1.75, 0, 1.0, 0]
#initial_conf[1] += np.pi / 6
initial_positions.update(zip(get_movable_joints(mbp, robot), initial_conf))
initial_poses = {
#brick: create_transform(translation=[0.6, 0, 0]),
brick: create_transform(translation=[0.4, 0, 0]),
}
goal_on = [
(brick, goal_surface),
]
task = Task(mbp, scene_graph, robot, gripper, movable=[brick], surfaces=surfaces, doors=doors,
initial_positions=initial_positions, initial_poses=initial_poses, goal_on=goal_on)
return mbp, scene_graph, task
def test_warnings_and_errors(self):
builder = DiagramBuilder()
sg = builder.AddSystem(SceneGraph())
v2 = builder.AddSystem(MeshcatVisualizer(scene_graph=sg))
builder.Connect(sg.get_query_output_port(),
v2.get_geometry_query_input_port())
v2.set_name("v2")
v4 = builder.AddSystem(MeshcatVisualizer(scene_graph=None))
builder.Connect(sg.get_query_output_port(),
v4.get_geometry_query_input_port())
v4.set_name("v4")
v5 = ConnectMeshcatVisualizer(builder, scene_graph=sg)
v5.set_name("v5")
v7 = ConnectMeshcatVisualizer(builder,
scene_graph=sg,
output_port=sg.get_query_output_port())
v7.set_name("v7")
with self.assertRaises(AssertionError):
v8 = ConnectMeshcatVisualizer(builder,
scene_graph=None,
output_port=None)
v8.set_name("v8")
v10 = ConnectMeshcatVisualizer(builder,
scene_graph=None,
output_port=sg.get_query_output_port())
v10.set_name("v10")
diagram = builder.Build()
context = diagram.CreateDefaultContext()
def PublishWithNoWarnings(v):
with warnings.catch_warnings(record=True) as w:
v.Publish(v.GetMyContextFromRoot(context))
self.assertEqual(len(w), 0, [x.message for x in w])
# Use geometry_query API
v2.load(v2.GetMyContextFromRoot(context))
PublishWithNoWarnings(v2)
v2.load()
PublishWithNoWarnings(v2)
# Use geometry_query API
v4.load(v4.GetMyContextFromRoot(context))
PublishWithNoWarnings(v4)
with self.assertRaises(RuntimeError):
v4.load() # Can't work without scene_graph nor context.
# Use geometry_query API
v5.load(v5.GetMyContextFromRoot(context))
PublishWithNoWarnings(v5)
v5.load()
PublishWithNoWarnings(v5)
# Use geometry_query API
v7.load(v7.GetMyContextFromRoot(context))
PublishWithNoWarnings(v7)
v7.load()
PublishWithNoWarnings(v7)
# Use geometry_query API
v10.load(v10.GetMyContextFromRoot(context))
PublishWithNoWarnings(v10)
with self.assertRaises(RuntimeError):
v10.load() # Can't work without scene_graph nor context.
def test_geometry(self):
builder = DiagramBuilder()
quadrotor = builder.AddSystem(QuadrotorPlant())
scene_graph = builder.AddSystem(SceneGraph())
state_port = quadrotor.get_output_port(0)
QuadrotorGeometry.AddToBuilder(builder, state_port, scene_graph)
from pydrake.systems.framework import DiagramBuilder
import argparse
parser = argparse.ArgumentParser()
parser.add_argument("--test",
action='store_true',
help="Causes the script to run without blocking for "
"user input.",
default=False)
args = parser.parse_args()
file_name = FindResourceOrThrow(
"drake/examples/multibody/cart_pole/cart_pole.sdf")
builder = DiagramBuilder()
scene_graph = builder.AddSystem(SceneGraph())
cart_pole = builder.AddSystem(MultibodyPlant())
cart_pole.RegisterAsSourceForSceneGraph(scene_graph)
AddModelFromSdfFile(file_name=file_name, plant=cart_pole)
cart_pole.AddForceElement(UniformGravityFieldElement([0, 0, -9.81]))
cart_pole.Finalize()
assert cart_pole.geometry_source_is_registered()
builder.Connect(scene_graph.get_query_output_port(),
cart_pole.get_geometry_query_input_port())
builder.Connect(cart_pole.get_geometry_poses_output_port(),
scene_graph.get_source_pose_port(cart_pole.get_source_id()))
builder.ExportInput(cart_pole.get_actuation_input_port())
ConnectDrakeVisualizer(builder=builder, scene_graph=scene_graph)
def load_tables(time_step=0.0, use_meshcat=True, use_controllers=True):
plant = MultibodyPlant(time_step=time_step)
scene_graph = SceneGraph()
# TODO: meshes aren't supported during collision checking
robot = AddModelFromSdfFile(file_name=IIWA14_SDF_PATH,
model_name='iiwa',
scene_graph=scene_graph,
plant=plant)
gripper = AddModelFromSdfFile(file_name=WSG50_SDF_PATH,
model_name='gripper',
scene_graph=scene_graph,
plant=plant) # TODO: sdf frame/link error
table = AddModelFromSdfFile(file_name=TABLE_SDF_PATH,
model_name='table',
scene_graph=scene_graph,
plant=plant)
table2 = AddModelFromSdfFile(file_name=TABLE_SDF_PATH,
model_name='table2',
scene_graph=scene_graph,
plant=plant)
sink = AddModelFromSdfFile(file_name=SINK_PATH,
model_name='sink',
scene_graph=scene_graph,
plant=plant)
stove = AddModelFromSdfFile(file_name=STOVE_PATH,
model_name='stove',
scene_graph=scene_graph,
plant=plant)
broccoli = AddModelFromSdfFile(file_name=BROCCOLI_PATH,
model_name='broccoli',
scene_graph=scene_graph,
plant=plant)
#wall = AddModelFromSdfFile(file_name=WALL_PATH, model_name='wall',
# scene_graph=scene_graph, plant=mbp)
wall = None
table2_x = 0.75
table_top_z = 0.7655 # TODO: use geometry
weld_gripper(plant, robot, gripper)
weld_to_world(plant, robot,
create_transform(translation=[0, 0, table_top_z]))
weld_to_world(plant, table, create_transform())
weld_to_world(plant, table2,
create_transform(translation=[table2_x, 0, 0]))
weld_to_world(plant, sink,
create_transform(translation=[table2_x, 0.25, table_top_z]))
weld_to_world(plant, stove,
create_transform(translation=[table2_x, -0.25, table_top_z]))
if wall is not None:
weld_to_world(
plant, wall,
create_transform(translation=[table2_x / 2, 0, table_top_z]))
plant.Finalize(scene_graph)
movable = [broccoli]
surfaces = [
Surface(plant, sink, 'base_link',
0), # Could also just pass the link index
Surface(plant, stove, 'base_link', 0),
]
initial_poses = {
broccoli: create_transform(translation=[table2_x, 0, table_top_z]),
}
diagram, state_machine = build_diagram(plant,
scene_graph,
robot,
gripper,
meshcat=use_meshcat,
controllers=use_controllers)
task = Task(diagram,
plant,
scene_graph,
robot,
gripper,
movable=movable,
surfaces=surfaces,
initial_poses=initial_poses,
goal_cooked=[broccoli])
return task, diagram, state_machine
