def MustardExampleSystem():
builder = DiagramBuilder()
# Create the physics engine + scene graph.
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=0.0)
parser = Parser(plant)
parser.package_map().Add("manipulation", FindResource("models"))
ProcessModelDirectives(
LoadModelDirectives(FindResource("models/mustard_w_cameras.yaml")),
plant, parser)
plant.Finalize()
# Add a visualizer just to help us see the object.
use_meshcat = False
if use_meshcat:
meshcat = builder.AddSystem(MeshcatVisualizer(scene_graph))
builder.Connect(scene_graph.get_pose_bundle_output_port(),
meshcat.get_input_port(0))
AddRgbdSensors(builder, plant, scene_graph)
diagram = builder.Build()
diagram.set_name("depth_camera_demo_system")
return diagram
def AddPlanarBinAndSimpleBox(plant,
mass=1.0,
mu=1.0,
width=0.2,
depth=0.05,
height=0.3):
parser = Parser(plant)
bin = parser.AddModelFromFile(FindResource("models/planar_bin.sdf"))
plant.WeldFrames(
plant.world_frame(), plant.GetFrameByName("bin_base", bin),
RigidTransform(RotationMatrix.MakeZRotation(np.pi / 2.0),
[0, 0, -0.015]))
planar_joint_frame = plant.AddFrame(
FixedOffsetFrame(
"planar_joint_frame", plant.world_frame(),
RigidTransform(RotationMatrix.MakeXRotation(np.pi / 2))))
# TODO(russt): make this a *random* box?
# TODO(russt): move random box to a shared py file.
box_instance = AddShape(plant, Box(width, depth, height), "box", mass, mu)
box_joint = plant.AddJoint(
PlanarJoint("box", planar_joint_frame,
plant.GetFrameByName("box", box_instance)))
box_joint.set_position_limits([-.5, -.1, -np.pi], [.5, .3, np.pi])
box_joint.set_velocity_limits([-2, -2, -2], [2, 2, 2])
box_joint.set_default_translation([0, depth / 2.0])
return box_instance
def AddCameraBox(plant, X_WC, name="camera0", parent_frame=None):
# TODO(russt): could be smarter and increment the default camera name (by
# checking with the plant).
if not parent_frame:
parent_frame = plant.world_frame()
camera = Parser(plant).AddModelFromFile(
FindResource("models/camera_box.sdf"), name)
plant.WeldFrames(parent_frame, plant.GetFrameByName("base", camera), X_WC)
def AddWsg(plant, iiwa_model_instance, roll=np.pi / 2.0, welded=False):
parser = Parser(plant)
if welded:
parser.package_map().Add(
"wsg_50_description",
os.path.dirname(
FindResourceOrThrow(
"drake/manipulation/models/wsg_50_description/package.xml"))
)
gripper = parser.AddModelFromFile(
FindResource("models/schunk_wsg_50_welded_fingers.sdf"), "gripper")
else:
gripper = parser.AddModelFromFile(
FindResourceOrThrow(
"drake/manipulation/models/"
"wsg_50_description/sdf/schunk_wsg_50_with_tip.sdf"))
X_7G = RigidTransform(RollPitchYaw(np.pi / 2.0, 0, roll), [0, 0, 0.114])
plant.WeldFrames(plant.GetFrameByName("iiwa_link_7", iiwa_model_instance),
plant.GetFrameByName("body", gripper), X_7G)
return gripper
def AddTwoLinkIiwa(plant, q0=[0.1, -1.2]):
urdf = FindResource("models/two_link_iiwa14.urdf")
parser = Parser(plant)
parser.package_map().Add(
"iiwa_description",
os.path.dirname(
FindResourceOrThrow(
"drake/manipulation/models/iiwa_description/package.xml")))
iiwa = parser.AddModelFromFile(urdf)
plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("iiwa_link_0"))
# Set default positions:
index = 0
for joint_index in plant.GetJointIndices(iiwa):
joint = plant.get_mutable_joint(joint_index)
if isinstance(joint, RevoluteJoint):
joint.set_default_angle(q0[index])
index += 1
return iiwa
import numpy as np
from pydrake.all import (DiagramBuilder, AddMultibodyPlantSceneGraph,
ConnectPlanarSceneGraphVisualizer, Parser,
PiecewisePolynomial)
from manipulation.utils import FindResource
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=0.0)
Parser(plant, scene_graph).AddModelFromFile(
FindResource("models/double_pendulum.urdf"))
plant.Finalize()
viz = ConnectPlanarSceneGraphVisualizer(builder,
scene_graph,
show=False,
xlim=[-.2, 2.2],
ylim=[-1., 1.])
viz.fig.set_size_inches([3, 2.5])
diagram = builder.Build()
context = diagram.CreateDefaultContext()
T = 2.
q = PiecewisePolynomial.FirstOrderHold(
[0, T], np.array([[-np.pi / 2.0 + 1., -np.pi / 2.0 - 1.], [-2., 2.]]))
plant_context = plant.GetMyContextFromRoot(context)
viz.start_recording()
for t in np.linspace(0, T, num=100):
context.SetTime(t)