def load(self):
"""
Loads all visualization elements in the Blender server.
@pre The `scene_graph` used to construct this object must be part of a
fully constructed diagram (e.g. via `DiagramBuilder.Build()`).
"""
self.bsi.send_remote_call("initialize_scene")
# Keeps track of registered bounding boxes, to keep us from
# having to register + delete brand new objects every cycle.
# If more than this number is needed in a given cycle,
# more are registered and this number is increased.
# If less are needed, the unused ones are made invisible.
# The attributes of all bounding boxes are updated every cycle.
self.num_registered_bounding_boxes = 0
self.num_visible_bounding_boxes = 0
# Intercept load message via mock LCM.
mock_lcm = DrakeMockLcm()
mock_lcm_subscriber = Subscriber(lcm=mock_lcm,
channel="DRAKE_VIEWER_LOAD_ROBOT",
lcm_type=lcmt_viewer_load_robot)
DispatchLoadMessage(self._scene_graph, mock_lcm)
mock_lcm.HandleSubscriptions(0)
assert mock_lcm_subscriber.count > 0
load_robot_msg = mock_lcm_subscriber.message
# Load all the elements over on the Blender side.
self.num_link_geometries_by_link_name = {}
self.link_subgeometry_local_tfs_by_link_name = {}
for i in range(load_robot_msg.num_links):
link = load_robot_msg.link[i]
[source_name, frame_name] = self._parse_name(link.name)
self.num_link_geometries_by_link_name[link.name] = link.num_geom
tfs = []
for j in range(link.num_geom):
geom = link.geom[j]
# MultibodyPlant currently sets alpha=0 to make collision
# geometry "invisible". Ignore those geometries here.
if geom.color[3] == 0:
continue
geom_name = self._format_geom_name(source_name, frame_name,
link.robot_num, j)
tfs.append(
RigidTransform(RotationMatrix(Quaternion(geom.quaternion)),
geom.position).GetAsMatrix4())
# It will have a material with this key.
# We will set it up after deciding whether it's
# a mesh with a texture or not...
material_key = "material_" + geom_name
material_key_assigned = False
# Check overrides
for override in self.material_overrides:
if override[0].match(geom_name):
print("Using override ", override[0].pattern,
" on name ", geom_name, " with applied args ",
override[1])
self.bsi.send_remote_call("register_material",
name=material_key,
**(override[1]))
material_key_assigned = True
if geom.type == geom.BOX:
assert geom.num_float_data == 3
# Blender cubes are 2x2x2 by default
do_load_geom = lambda: self.bsi.send_remote_call(
"register_object",
name="obj_" + geom_name,
type="cube",
scale=[x * 0.5 for x in geom.float_data[:3]],
location=geom.position,
quaternion=geom.quaternion,
material=material_key)
elif geom.type == geom.SPHERE:
assert geom.num_float_data == 1
do_load_geom = lambda: self.bsi.send_remote_call(
"register_object",
name="obj_" + geom_name,
type="sphere",
scale=geom.float_data[0],
location=geom.position,
quaternion=geom.quaternion,
material=material_key)
elif geom.type == geom.CYLINDER:
assert geom.num_float_data == 2
# Blender cylinders are r=1, h=2 by default
do_load_geom = lambda: self.bsi.send_remote_call(
"register_object",
name="obj_" + geom_name,
type="cylinder",
scale=[
geom.float_data[0], geom.float_data[0], 0.5 * geom.
float_data[1]
],
location=geom.position,
quaternion=geom.quaternion,
material=material_key)
elif geom.type == geom.MESH:
do_load_geom = lambda: self.bsi.send_remote_call(
"register_object",
name="obj_" + geom_name,
type="obj",
path=geom.string_data[0:-3] + "obj",
scale=geom.float_data[:3],
location=geom.position,
quaternion=geom.quaternion,
material=material_key)
# Attempt to find a texture for the object by looking for an
# identically-named *.png next to the model.
# TODO(gizatt): In the long term, this kind of material information
# should be gleaned from the SceneGraph constituents themselves, so
# that we visualize what the simulation is *actually* reasoning about
# rather than what files happen to be present.
candidate_texture_path_png = geom.string_data[0:-3] + "png"
if not material_key_assigned and os.path.exists(
candidate_texture_path_png):
material_key_assigned = self.bsi.send_remote_call(
"register_material",
name=material_key,
material_type="color_texture",
path=candidate_texture_path_png)
else:
print("UNSUPPORTED GEOMETRY TYPE {} IGNORED".format(
geom.type))
continue
if not material_key_assigned:
material_key_assigned = self.bsi.send_remote_call(
"register_material",
name=material_key,
material_type="color",
color=geom.color[:4])
# Finally actually load the geometry now that the material
# is registered.
do_load_geom()
self.link_subgeometry_local_tfs_by_link_name[link.name] = tfs
for i, camera_tf in enumerate(self.camera_tfs):
camera_tf_post = self.global_transform.multiply(camera_tf)
self.bsi.send_remote_call(
"register_camera",
name="cam_%d" % i,
location=camera_tf_post.translation().tolist(),
quaternion=camera_tf_post.quaternion().wxyz().tolist(),
angle=90)
self.bsi.send_remote_call("configure_rendering",
camera_name='cam_%d' % i,
resolution=[640 * 2, 480 * 2],
file_format="JPEG")
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
options.random_seed = 1314
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 test_contact_force(self):
"""A block sitting on a table."""
object_file_path = FindResourceOrThrow(
"drake/examples/manipulation_station/models/061_foam_brick.sdf")
table_file_path = FindResourceOrThrow(
"drake/examples/kuka_iiwa_arm/models/table/"
"extra_heavy_duty_table_surface_only_collision.sdf")
# T: tabletop frame.
X_TObject = RigidTransform([0, 0, 0.2])
builder = DiagramBuilder()
plant = MultibodyPlant(0.002)
_, scene_graph = AddMultibodyPlantSceneGraph(builder, plant)
object_model = Parser(plant=plant).AddModelFromFile(object_file_path)
table_model = Parser(plant=plant).AddModelFromFile(table_file_path)
# Weld table to world.
plant.WeldFrames(A=plant.world_frame(),
B=plant.GetFrameByName("link", table_model))
plant.Finalize()
# Add meshcat visualizer.
viz = builder.AddSystem(
MeshcatVisualizer(scene_graph, zmq_url=ZMQ_URL,
open_browser=False))
builder.Connect(scene_graph.get_pose_bundle_output_port(),
viz.get_input_port(0))
# Add contact visualizer.
contact_viz = builder.AddSystem(
MeshcatContactVisualizer(meshcat_viz=viz,
force_threshold=0,
contact_force_scale=10,
plant=plant))
contact_input_port = contact_viz.GetInputPort("contact_results")
builder.Connect(plant.GetOutputPort("contact_results"),
contact_input_port)
builder.Connect(scene_graph.get_pose_bundle_output_port(),
contact_viz.GetInputPort("pose_bundle"))
diagram = builder.Build()
diagram_context = diagram.CreateDefaultContext()
mbp_context = diagram.GetMutableSubsystemContext(
plant, diagram_context)
X_WT = plant.CalcRelativeTransform(mbp_context, plant.world_frame(),
plant.GetFrameByName("top_center"))
plant.SetFreeBodyPose(mbp_context,
plant.GetBodyByName("base_link", object_model),
X_WT.multiply(X_TObject))
simulator = Simulator(diagram, diagram_context)
simulator.set_publish_every_time_step(False)
simulator.AdvanceTo(1.0)
contact_viz_context = (diagram.GetMutableSubsystemContext(
contact_viz, diagram_context))
contact_results = contact_viz.EvalAbstractInput(
contact_viz_context, contact_input_port.get_index()).get_value()
self.assertGreater(contact_results.num_point_pair_contacts(), 0)
self.assertEqual(contact_viz._contact_key_counter, 4)
def test_rgbd_sensor(self):
def check_ports(system):
self.assertIsInstance(system.query_object_input_port(), InputPort)
self.assertIsInstance(system.color_image_output_port(), OutputPort)
self.assertIsInstance(system.depth_image_32F_output_port(),
OutputPort)
self.assertIsInstance(system.depth_image_16U_output_port(),
OutputPort)
self.assertIsInstance(system.label_image_output_port(), OutputPort)
self.assertIsInstance(system.body_pose_in_world_output_port(),
OutputPort)
# Use HDTV size.
width = 1280
height = 720
# There are *two* variants of the constructor for each camera
# representation: one with color and depth explicitly specified and one
# with only depth. We enumerate all four here.
def construct(parent_id, X_PB):
color_camera = ColorRenderCamera(
RenderCameraCore("renderer",
mut.CameraInfo(width, height, np.pi / 6),
ClippingRange(0.1, 6.0), RigidTransform()),
False)
depth_camera = DepthRenderCamera(color_camera.core(),
DepthRange(0.1, 5.5))
return mut.RgbdSensor(parent_id=parent_id,
X_PB=X_PB,
color_camera=color_camera,
depth_camera=depth_camera)
def construct_single(parent_id, X_PB):
depth_camera = DepthRenderCamera(
RenderCameraCore("renderer",
mut.CameraInfo(width, height, np.pi / 6),
ClippingRange(0.1, 6.0), RigidTransform()),
DepthRange(0.1, 5.5))
return mut.RgbdSensor(parent_id=parent_id,
X_PB=X_PB,
depth_camera=depth_camera)
# Put it at the origin.
X_WB = RigidTransform()
# This id would fail if we tried to render; no such id exists.
parent_id = FrameId.get_new_id()
def check_info(camera_info):
self.assertIsInstance(camera_info, mut.CameraInfo)
self.assertEqual(camera_info.width(), width)
self.assertEqual(camera_info.height(), height)
for constructor in [construct, construct_single]:
sensor = constructor(parent_id, X_WB)
check_info(sensor.color_camera_info())
check_info(sensor.depth_camera_info())
self.assertIsInstance(sensor.X_BC(), RigidTransform)
self.assertIsInstance(sensor.X_BD(), RigidTransform)
self.assertEqual(sensor.parent_frame_id(), parent_id)
check_ports(sensor)
# Test discrete camera. We'll simply use the last sensor constructed.
period = mut.RgbdSensorDiscrete.kDefaultPeriod
discrete = mut.RgbdSensorDiscrete(sensor=sensor,
period=period,
render_label_image=True)
self.assertTrue(discrete.sensor() is sensor)
self.assertEqual(discrete.period(), period)
check_ports(discrete)
# That we can access the state as images.
context = discrete.CreateDefaultContext()
values = context.get_abstract_state()
self.assertIsInstance(values.get_value(0), Value[mut.ImageRgba8U])
self.assertIsInstance(values.get_value(1), Value[mut.ImageDepth32F])
self.assertIsInstance(values.get_value(2), Value[mut.ImageDepth16U])
self.assertIsInstance(values.get_value(3), Value[mut.ImageLabel16I])
def _build_body_patches(self, use_random_colors,
substitute_collocated_mesh_files):
"""
Generates body patches. self._patch_Blist stores a list of patches for
each body (starting at body id 1). A body patch is a list of all 3D
vertices of a piece of visual geometry.
"""
self._patch_Blist = {}
self._patch_Blist_colors = {}
memq_lcm = DrakeLcm("memq://")
memq_lcm_subscriber = Subscriber(lcm=memq_lcm,
channel="DRAKE_VIEWER_LOAD_ROBOT",
lcm_type=lcmt_viewer_load_robot)
# TODO(SeanCurtis-TRI): Use SceneGraph inspection instead of mocking
# LCM and inspecting the generated message.
DispatchLoadMessage(self._scene_graph, memq_lcm)
memq_lcm.HandleSubscriptions(0)
assert memq_lcm_subscriber.count > 0
load_robot_msg = memq_lcm_subscriber.message
# Spawn a random color generator, in case we need to pick random colors
# for some bodies. Each body will be given a unique color when using
# this random generator, with each visual element of the body colored
# the same.
color = iter(
plt.cm.rainbow(np.linspace(0, 1, load_robot_msg.num_links)))
for i in range(load_robot_msg.num_links):
link = load_robot_msg.link[i]
this_body_patches = []
this_body_colors = []
this_color = next(color)
for j in range(link.num_geom):
geom = link.geom[j]
# MultibodyPlant currently sets alpha=0 to make collision
# geometry "invisible". Ignore those geometries here.
if geom.color[3] == 0:
continue
X_BG = RigidTransform(
RotationMatrix(Quaternion(geom.quaternion)), geom.position)
if geom.type == geom.BOX:
assert geom.num_float_data == 3
# Draw a bounding box.
patch_G = np.vstack(
(geom.float_data[0] / 2. *
np.array([-1, -1, 1, 1, -1, -1, 1, 1]),
geom.float_data[1] / 2. *
np.array([-1, 1, -1, 1, -1, 1, -1, 1]),
geom.float_data[2] / 2. *
np.array([-1, -1, -1, -1, 1, 1, 1, 1])))
elif geom.type == geom.SPHERE:
assert geom.num_float_data == 1
radius = geom.float_data[0]
lati, longi = np.meshgrid(np.arange(0., 2. * math.pi, 0.5),
np.arange(0., 2. * math.pi, 0.5))
lati = lati.ravel()
longi = longi.ravel()
patch_G = np.vstack([
np.sin(lati) * np.cos(longi),
np.sin(lati) * np.sin(longi),
np.cos(lati)
])
patch_G *= radius
elif geom.type == geom.CYLINDER:
assert geom.num_float_data == 2
radius = geom.float_data[0]
length = geom.float_data[1]
# In the lcm geometry, cylinders are along +z
# https://github.com/RobotLocomotion/drake/blob/last_sha_with_original_matlab/drake/matlab/systems/plants/RigidBodyCylinder.m
# Two circles: one at bottom, one at top.
sample_pts = np.arange(0., 2. * math.pi, 0.25)
patch_G = np.hstack([
np.array([[
radius * math.cos(pt), radius * math.sin(pt),
-length / 2.
],
[
radius * math.cos(pt),
radius * math.sin(pt), length / 2.
]]).T for pt in sample_pts
])
elif geom.type == geom.MESH:
filename = geom.string_data
base, ext = os.path.splitext(filename)
if (ext.lower() != ".obj"
and substitute_collocated_mesh_files):
# Check for a co-located .obj file (case insensitive).
for f in glob.glob(base + '.*'):
if f[-4:].lower() == '.obj':
filename = f
break
if filename[-4:].lower() != '.obj':
raise RuntimeError("The given file " + filename +
" is not "
"supported and no alternate " +
base + ".obj could be found.")
if not os.path.exists(filename):
raise FileNotFoundError(errno.ENOENT,
os.strerror(errno.ENOENT),
filename)
mesh = ReadObjToSurfaceMesh(filename)
patch_G = np.vstack([v.r_MV() for v in mesh.vertices()]).T
else:
print("UNSUPPORTED GEOMETRY TYPE {} IGNORED".format(
geom.type))
continue
# Compute pose in body.
patch_B = X_BG @ patch_G
# Close path if not closed.
if (patch_B[:, -1] != patch_B[:, 0]).any():
patch_B = np.hstack((patch_B, patch_B[:, 0][np.newaxis].T))
this_body_patches.append(patch_B)
if use_random_colors:
this_body_colors.append(this_color)
else:
this_body_colors.append(geom.color)
self._patch_Blist[link.name] = this_body_patches
self._patch_Blist_colors[link.name] = this_body_colors
def test_query_object(self, T):
RigidTransform = RigidTransform_[float]
SceneGraph = mut.SceneGraph_[T]
QueryObject = mut.QueryObject_[T]
SceneGraphInspector = mut.SceneGraphInspector_[T]
FramePoseVector = mut.FramePoseVector_[T]
# First, ensure we can default-construct it.
model = QueryObject()
self.assertIsInstance(model, QueryObject)
scene_graph = SceneGraph()
source_id = scene_graph.RegisterSource("source")
frame_id = scene_graph.RegisterFrame(
source_id=source_id, frame=mut.GeometryFrame("frame"))
geometry_id = scene_graph.RegisterGeometry(
source_id=source_id, frame_id=frame_id,
geometry=mut.GeometryInstance(X_PG=RigidTransform(),
shape=mut.Sphere(1.), name="sphere"))
render_params = mut.render.RenderEngineVtkParams()
renderer_name = "test_renderer"
scene_graph.AddRenderer(renderer_name,
mut.render.MakeRenderEngineVtk(render_params))
context = scene_graph.CreateDefaultContext()
pose_vector = FramePoseVector()
pose_vector.set_value(frame_id, RigidTransform_[T]())
scene_graph.get_source_pose_port(source_id).FixValue(
context, pose_vector)
query_object = scene_graph.get_query_output_port().Eval(context)
self.assertIsInstance(query_object.inspector(), SceneGraphInspector)
with catch_drake_warnings(expected_count=3):
self.assertIsInstance(
query_object.X_WF(id=frame_id), RigidTransform_[T])
self.assertIsInstance(
query_object.X_PF(id=frame_id), RigidTransform_[T])
self.assertIsInstance(
query_object.X_WG(id=geometry_id), RigidTransform_[T])
self.assertIsInstance(
query_object.GetPoseInWorld(frame_id=frame_id), RigidTransform_[T])
self.assertIsInstance(
query_object.GetPoseInParent(frame_id=frame_id),
RigidTransform_[T])
self.assertIsInstance(
query_object.GetPoseInWorld(geometry_id=geometry_id),
RigidTransform_[T])
# Proximity queries -- all of these will produce empty results.
results = query_object.ComputeSignedDistancePairwiseClosestPoints()
self.assertEqual(len(results), 0)
results = query_object.ComputePointPairPenetration()
self.assertEqual(len(results), 0)
results = query_object.ComputeSignedDistanceToPoint(p_WQ=(1, 2, 3))
self.assertEqual(len(results), 0)
results = query_object.FindCollisionCandidates()
self.assertEqual(len(results), 0)
self.assertFalse(query_object.HasCollisions())
# ComputeSignedDistancePairClosestPoints() requires two valid geometry
# ids. There are none in this SceneGraph instance. Rather than
# populating the SceneGraph, we look for the exception thrown in
# response to invalid ids as evidence of correct binding.
with self.assertRaisesRegex(
RuntimeError,
r"The geometry given by id \d+ does not reference a geometry"
+ " that can be used in a signed distance query"):
query_object.ComputeSignedDistancePairClosestPoints(
geometry_id_A=mut.GeometryId.get_new_id(),
geometry_id_B=mut.GeometryId.get_new_id())
# TODO(SeanCurtis-TRI) Remove this call at the same time as deprecating
# the subsequent deprecation test; it is only here to show that the
# non-keyword call invokes the non-deprecated overload.
self.assertRaisesRegex(
RuntimeError,
r"The geometry given by id \d+ does not reference a geometry"
+ " that can be used in a signed distance query",
query_object.ComputeSignedDistancePairClosestPoints,
mut.GeometryId.get_new_id(), mut.GeometryId.get_new_id())
with catch_drake_warnings(expected_count=1):
with self.assertRaisesRegex(
RuntimeError,
r"The geometry given by id \d+ does not reference a geometry"
+ " that can be used in a signed distance query"):
query_object.ComputeSignedDistancePairClosestPoints(
id_A=mut.GeometryId.get_new_id(),
id_B=mut.GeometryId.get_new_id())
# Confirm rendering API returns images of appropriate type.
camera_core = mut.render.RenderCameraCore(
renderer_name=renderer_name,
intrinsics=CameraInfo(width=10, height=10, fov_y=pi/6),
clipping=mut.render.ClippingRange(0.1, 10.0),
X_BS=RigidTransform())
color_camera = mut.render.ColorRenderCamera(
core=camera_core, show_window=False)
depth_camera = mut.render.DepthRenderCamera(
core=camera_core, depth_range=mut.render.DepthRange(0.1, 5.0))
image = query_object.RenderColorImage(
camera=color_camera, parent_frame=SceneGraph.world_frame_id(),
X_PC=RigidTransform())
self.assertIsInstance(image, ImageRgba8U)
image = query_object.RenderDepthImage(
camera=depth_camera, parent_frame=SceneGraph.world_frame_id(),
X_PC=RigidTransform())
self.assertIsInstance(image, ImageDepth32F)
image = query_object.RenderLabelImage(
camera=color_camera, parent_frame=SceneGraph.world_frame_id(),
X_PC=RigidTransform())
self.assertIsInstance(image, ImageLabel16I)
with catch_drake_warnings(expected_count=4):
# One deprecation warning for constructing DepthCameraProperties
# and one for each invocation of Render*Image.
depth_camera = mut.render.DepthCameraProperties(
width=320, height=240, fov_y=pi/6, renderer_name=renderer_name,
z_near=0.1, z_far=5.0)
image = query_object.RenderColorImage(
camera=depth_camera, parent_frame=SceneGraph.world_frame_id(),
X_PC=RigidTransform())
self.assertIsInstance(image, ImageRgba8U)
image = query_object.RenderDepthImage(
camera=depth_camera, parent_frame=SceneGraph.world_frame_id(),
X_PC=RigidTransform())
self.assertIsInstance(image, ImageDepth32F)
image = query_object.RenderLabelImage(
camera=depth_camera, parent_frame=SceneGraph.world_frame_id(),
X_PC=RigidTransform())
self.assertIsInstance(image, ImageLabel16I)
with open(os.path.join(this_dir, 'ur10.sdf.in'), 'r') as file_in:
with open(sdf_file_path, 'w') as file_out:
file_out.write(file_in.read().replace('PWD_GOES_HERE', this_dir))
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=0.001)
parser = Parser(plant)
model_name = "ur10"
model = parser.AddModelFromFile(sdf_file_path, model_name)
# Weld to world so it doesn't fall through floor :D
base_frame = plant.GetFrameByName("base", model)
X_WB = RigidTransform([0, 0, 0])
plant.WeldFrames(plant.world_frame(), base_frame, X_WB)
plant.Finalize()
# Must happen after Finalize
# RuntimeError: Pre-finalize calls to 'num_actuated_dofs()' are not allowed; you must call Finalize() first.
no_control(plant, builder, model)
joints = []
for i in range(plant.num_joints()):
joints.append(plant.get_joint(JointIndex(i)))
rclpy.init()
node = rclpy.create_node('drake_demo')
def __init__(self,
visible=Visible(),
min_range=MinRange(),
max_range=MaxRange(),
value=Value()):
"""
Args:
visible: An object with boolean elements for 'roll', 'pitch',
'yaw', 'x', 'y', 'z'; the intention is for this to be the
PoseSliders.Visible() namedtuple. Defaults to all true.
min_range, max_range, value: Objects with float values for 'roll',
'pitch', 'yaw', 'x', 'y', 'z'; the intention is for the
caller to use the PoseSliders.MinRange, MaxRange, and
Value namedtuples. See those tuples for default values.
"""
LeafSystem.__init__(self)
self.DeclareAbstractOutputPort(
"pose", lambda: AbstractValue.Make(RigidTransform()),
self.DoCalcOutput)
# Note: This timing affects the keyboard teleop performance. A larger
# time step causes more lag in the response.
self.DeclarePeriodicPublish(0.01, 0.0)
self._roll = FloatSlider(min=min_range.roll,
max=max_range.roll,
value=value.roll,
step=0.01,
continuous_update=True,
description="roll",
layout=Layout(width='90%'))
self._pitch = FloatSlider(min=min_range.pitch,
max=max_range.pitch,
value=value.pitch,
step=0.01,
continuous_update=True,
description="pitch",
layout=Layout(width='90%'))
self._yaw = FloatSlider(min=min_range.yaw,
max=max_range.yaw,
value=value.yaw,
step=0.01,
continuous_update=True,
description="yaw",
layout=Layout(width='90%'))
self._x = FloatSlider(min=min_range.x,
max=max_range.x,
value=value.x,
step=0.01,
continuous_update=True,
description="x",
orient='horizontal',
layout=Layout(width='90%'))
self._y = FloatSlider(min=min_range.y,
max=max_range.y,
value=value.y,
step=0.01,
continuous_update=True,
description="y",
layout=Layout(width='90%'))
self._z = FloatSlider(min=min_range.z,
max=max_range.z,
value=value.z,
step=0.01,
continuous_update=True,
description="z",
layout=Layout(width='90%'))
if visible.roll:
display(self._roll)
if visible.pitch:
display(self._pitch)
if visible.yaw:
display(self._yaw)
if visible.x:
display(self._x)
if visible.y:
display(self._y)
if visible.z:
display(self._z)
def se3_from_xyz(xyz):
return RigidTransform(p=xyz)
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(
"--duration", type=float, default=np.inf,
help="Desired duration of the simulation in seconds.")
parser.add_argument(
"--hardware", action='store_true',
help="Use the ManipulationStationHardwareInterface instead of an "
"in-process simulation.")
parser.add_argument(
"--test", action='store_true',
help="Disable opening the gui window for testing.")
parser.add_argument(
'--setup', type=str, default='manipulation_class',
help="The manipulation station setup to simulate. ",
choices=['manipulation_class', 'clutter_clearing', 'planar'])
MeshcatVisualizer.add_argparse_argument(parser)
args = parser.parse_args()
builder = DiagramBuilder()
if args.hardware:
# TODO(russt): Replace this hard-coded camera serial number with a
# config file.
camera_ids = ["805212060544"]
station = builder.AddSystem(ManipulationStationHardwareInterface(
camera_ids))
station.Connect(wait_for_cameras=False)
else:
station = builder.AddSystem(ManipulationStation())
# Initializes the chosen station type.
if args.setup == 'manipulation_class':
station.SetupManipulationClassStation()
station.AddManipulandFromFile(
"drake/examples/manipulation_station/models/"
+ "061_foam_brick.sdf",
RigidTransform(RotationMatrix.Identity(), [0.6, 0, 0]))
elif args.setup == 'clutter_clearing':
station.SetupClutterClearingStation()
ycb_objects = CreateClutterClearingYcbObjectList()
for model_file, X_WObject in ycb_objects:
station.AddManipulandFromFile(model_file, X_WObject)
elif args.setup == 'planar':
station.SetupPlanarIiwaStation()
station.AddManipulandFromFile(
"drake/examples/manipulation_station/models/"
+ "061_foam_brick.sdf",
RigidTransform(RotationMatrix.Identity(), [0.6, 0, 0]))
station.Finalize()
ConnectDrakeVisualizer(builder, station.get_scene_graph(),
station.GetOutputPort("pose_bundle"))
if args.meshcat:
meshcat = builder.AddSystem(MeshcatVisualizer(
station.get_scene_graph(), zmq_url=args.meshcat,
open_browser=args.open_browser))
builder.Connect(station.GetOutputPort("pose_bundle"),
meshcat.get_input_port(0))
if args.setup == 'planar':
meshcat.set_planar_viewpoint()
if args.setup == 'planar':
pyplot_visualizer = builder.AddSystem(PlanarSceneGraphVisualizer(
station.get_scene_graph()))
builder.Connect(station.GetOutputPort("pose_bundle"),
pyplot_visualizer.get_input_port(0))
teleop = builder.AddSystem(JointSliders(station.get_controller_plant(),
length=800))
if args.test:
teleop.window.withdraw() # Don't display the window when testing.
num_iiwa_joints = station.num_iiwa_joints()
filter = builder.AddSystem(FirstOrderLowPassFilter(
time_constant=2.0, size=num_iiwa_joints))
builder.Connect(teleop.get_output_port(0), filter.get_input_port(0))
builder.Connect(filter.get_output_port(0),
station.GetInputPort("iiwa_position"))
wsg_buttons = builder.AddSystem(SchunkWsgButtons(teleop.window))
builder.Connect(wsg_buttons.GetOutputPort("position"),
station.GetInputPort("wsg_position"))
builder.Connect(wsg_buttons.GetOutputPort("force_limit"),
station.GetInputPort("wsg_force_limit"))
# When in regression test mode, log our joint velocities to later check
# that they were sufficiently quiet.
if args.test:
iiwa_velocities = builder.AddSystem(SignalLogger(num_iiwa_joints))
builder.Connect(station.GetOutputPort("iiwa_velocity_estimated"),
iiwa_velocities.get_input_port(0))
else:
iiwa_velocities = None
diagram = builder.Build()
simulator = Simulator(diagram)
# This is important to avoid duplicate publishes to the hardware interface:
simulator.set_publish_every_time_step(False)
station_context = diagram.GetMutableSubsystemContext(
station, simulator.get_mutable_context())
station.GetInputPort("iiwa_feedforward_torque").FixValue(
station_context, np.zeros(num_iiwa_joints))
# If the diagram is only the hardware interface, then we must advance it a
# little bit so that first LCM messages get processed. A simulated plant is
# already publishing correct positions even without advancing, and indeed
# we must not advance a simulated plant until the sliders and filters have
# been initialized to match the plant.
if args.hardware:
simulator.AdvanceTo(1e-6)
# Eval the output port once to read the initial positions of the IIWA.
q0 = station.GetOutputPort("iiwa_position_measured").Eval(
station_context)
teleop.set_position(q0)
filter.set_initial_output_value(diagram.GetMutableSubsystemContext(
filter, simulator.get_mutable_context()), q0)
simulator.set_target_realtime_rate(args.target_realtime_rate)
simulator.AdvanceTo(args.duration)
# Ensure that our initialization logic was correct, by inspecting our
# logged joint velocities.
if args.test:
for time, qdot in zip(iiwa_velocities.sample_times(),
iiwa_velocities.data().transpose()):
# TODO(jwnimmer-tri) We should be able to do better than a 40
# rad/sec limit, but that's the best we can enforce for now.
if qdot.max() > 0.1:
print(f"ERROR: large qdot {qdot} at time {time}")
sys.exit(1)
def value_to_pose(val):
return RigidTransform(RollPitchYaw(val.roll, val.pitch, val.yaw),
[val.x, val.y, val.z])
def add_triad(
source_id,
frame_id,
scene_graph,
*,
length,
radius,
opacity,
X_FT=RigidTransform(),
name="frame",
):
"""
Adds illustration geometry representing the coordinate frame, with the
x-axis drawn in red, the y-axis in green and the z-axis in blue. The axes
point in +x, +y and +z directions, respectively.
Based on [code permalink](https://github.com/RussTedrake/manipulation/blob/5e59811/manipulation/scenarios.py#L367-L414).# noqa
Args:
source_id: The source registered with SceneGraph.
frame_id: A geometry::frame_id registered with scene_graph.
scene_graph: The SceneGraph with which we will register the geometry.
length: the length of each axis in meters.
radius: the radius of each axis in meters.
opacity: the opacity of the coordinate axes, between 0 and 1.
X_FT: a RigidTransform from the triad frame T to the frame_id frame F
name: the added geometry will have names name + " x-axis", etc.
"""
# x-axis
X_TG = RigidTransform(
RotationMatrix.MakeYRotation(np.pi / 2),
[length / 2.0, 0, 0],
)
geom = GeometryInstance(
X_FT.multiply(X_TG), Cylinder(radius, length), name + " x-axis"
)
geom.set_illustration_properties(
MakePhongIllustrationProperties([1, 0, 0, opacity])
)
scene_graph.RegisterGeometry(source_id, frame_id, geom)
# y-axis
X_TG = RigidTransform(
RotationMatrix.MakeXRotation(np.pi / 2),
[0, length / 2.0, 0],
)
geom = GeometryInstance(
X_FT.multiply(X_TG), Cylinder(radius, length), name + " y-axis"
)
geom.set_illustration_properties(
MakePhongIllustrationProperties([0, 1, 0, opacity])
)
scene_graph.RegisterGeometry(source_id, frame_id, geom)
# z-axis
X_TG = RigidTransform([0, 0, length / 2.0])
geom = GeometryInstance(
X_FT.multiply(X_TG), Cylinder(radius, length), name + " z-axis"
)
geom.set_illustration_properties(
MakePhongIllustrationProperties([0, 0, 1, opacity])
)
scene_graph.RegisterGeometry(source_id, frame_id, geom)
def DepthCameraDemoSystem():
builder = DiagramBuilder()
# If you have trouble finding resources, you can enable trace logging
# to see how `FindResource*` is searching.
set_log_level("trace")
# Create the physics engine + scene graph.
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=0.0)
# Add a single object into it.
Parser(plant, scene_graph).AddModelFromFile(FindResourceOrThrow(
"drake/manipulation/models/ycb/sdf/006_mustard_bottle.sdf"))
# Add a rendering engine
renderer = "my_renderer"
scene_graph.AddRenderer(renderer,
MakeRenderEngineVtk(RenderEngineVtkParams()))
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))
# Add a camera to the environment.
pose = RigidTransform(RollPitchYaw(-0.2, 0.2, 0), [-.1, -0.1, -.5])
properties = DepthCameraProperties(
width=640,
height=480,
fov_y=np.pi / 4.0,
renderer_name=renderer,
z_near=0.1,
z_far=10.0)
camera = builder.AddSystem(RgbdSensor(
parent_id=scene_graph.world_frame_id(), X_PB=pose,
properties=properties, show_window=False))
camera.set_name("rgbd_sensor")
builder.Connect(scene_graph.get_query_output_port(),
camera.query_object_input_port())
# Export the camera outputs
builder.ExportOutput(camera.color_image_output_port(), "color_image")
builder.ExportOutput(camera.depth_image_32F_output_port(), "depth_image")
# Add a system to convert the camera output into a point cloud
to_point_cloud = builder.AddSystem(
DepthImageToPointCloud(
camera_info=camera.depth_camera_info(),
fields=BaseField.kXYZs | BaseField.kRGBs))
builder.Connect(
camera.depth_image_32F_output_port(),
to_point_cloud.depth_image_input_port())
builder.Connect(
camera.color_image_output_port(),
to_point_cloud.color_image_input_port())
# Export the point cloud output.
builder.ExportOutput(
to_point_cloud.point_cloud_output_port(),
"point_cloud")
diagram = builder.Build()
diagram.set_name("depth_camera_demo_system")
return diagram
def buildViewPatches(self, use_random_colors):
''' Generates view patches. self.viewPatches stores a list of
viewPatches for each body (starting at body id 1). A viewPatch is a
list of all 3D vertices of a piece of visual geometry. '''
self.viewPatches = {}
self.viewPatchColors = {}
mock_lcm = DrakeMockLcm()
mock_lcm_subscriber = Subscriber(lcm=mock_lcm,
channel="DRAKE_VIEWER_LOAD_ROBOT",
lcm_type=lcmt_viewer_load_robot)
DispatchLoadMessage(self._scene_graph, mock_lcm)
mock_lcm.HandleSubscriptions(0)
assert mock_lcm_subscriber.count > 0
load_robot_msg = mock_lcm_subscriber.message
# Spawn a random color generator, in case we need to pick
# random colors for some bodies. Each body will be given
# a unique color when using this random generator, with
# each visual element of the body colored the same.
color = iter(plt.cm.rainbow(np.linspace(0, 1,
load_robot_msg.num_links)))
for i in range(load_robot_msg.num_links):
link = load_robot_msg.link[i]
this_body_patches = []
this_body_colors = []
this_color = next(color)
for j in range(link.num_geom):
geom = link.geom[j]
# MultibodyPlant currently sets alpha=0 to make collision
# geometry "invisible". Ignore those geometries here.
if geom.color[3] == 0:
continue
element_local_tf = RigidTransform(
RotationMatrix(Quaternion(geom.quaternion)),
geom.position)
if geom.type == geom.BOX:
assert geom.num_float_data == 3
# Draw a bounding box.
patch = np.vstack((
geom.float_data[0]/2.*np.array(
[-1, -1, 1, 1, -1, -1, 1, 1]),
geom.float_data[1]/2.*np.array(
[-1, 1, -1, 1, -1, 1, -1, 1]),
geom.float_data[2]/2.*np.array(
[-1, -1, -1, -1, 1, 1, 1, 1])))
elif geom.type == geom.SPHERE:
assert geom.num_float_data == 1
radius = geom.float_data[0]
lati, longi = np.meshgrid(np.arange(0., 2.*math.pi, 0.5),
np.arange(0., 2.*math.pi, 0.5))
lati = lati.ravel()
longi = longi.ravel()
patch = np.vstack([
np.sin(longi)*np.cos(lati),
np.sin(longi)*np.sin(lati),
np.cos(lati)])
patch *= radius
elif geom.type == geom.CYLINDER:
assert geom.num_float_data == 2
radius = geom.float_data[0]
length = geom.float_data[1]
# In the lcm geometry, cylinders are along +z
# https://github.com/RobotLocomotion/drake/blob/last_sha_with_original_matlab/drake/matlab/systems/plants/RigidBodyCylinder.m
# Two circles: one at bottom, one at top.
sample_pts = np.arange(0., 2.*math.pi, 0.25)
patch = np.hstack(
[np.array([
[radius*math.cos(pt),
radius*math.sin(pt),
-length/2.],
[radius*math.cos(pt),
radius*math.sin(pt),
length/2.]]).T
for pt in sample_pts])
elif geom.type == geom.MESH:
# TODO(gizatt): Remove trimesh and shapely dependency when
# vertex information is accessible from the SceneGraph
# interface.
mesh = trimesh.load(geom.string_data)
patch = mesh.vertices.T
# Apply scaling
for i in range(3):
patch[i, :] *= geom.float_data[i]
else:
print("UNSUPPORTED GEOMETRY TYPE {} IGNORED".format(
geom.type))
continue
patch = np.vstack((patch, np.ones((1, patch.shape[1]))))
patch = np.dot(element_local_tf.GetAsMatrix4(), patch)
# Close path if not closed
if (patch[:, -1] != patch[:, 0]).any():
patch = np.hstack((patch, patch[:, 0][np.newaxis].T))
this_body_patches.append(patch)
if use_random_colors:
this_body_colors.append(this_color)
else:
this_body_colors.append(geom.color)
self.viewPatches[link.name] = this_body_patches
self.viewPatchColors[link.name] = this_body_colors
def set_atlas_initial_pose(plant, plant_context):
pelvis = plant.GetBodyByName("pelvis")
X_WP = RigidTransform(RollPitchYaw(0.0, 0.0, 0.0), np.array([0.0, 0.0, 0.94]))
plant.SetFreeBodyPose(plant_context, pelvis, X_WP)
def construct_environment(masses: typing.List, box_sizes: typing.List):
"""
Construct an environment with many free boxes.
@param masses masses[i] is the mass of box i.
@param box_sizes box_sizes[i] is the size of box i.
"""
builder = DiagramBuilder_[AutoDiffXd]()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, 0.0)
# Add the ground as a big box.
ground_box = plant.AddRigidBody(
"ground", SpatialInertia(1, np.array([0, 0, 0]), UnitInertia(1, 1, 1)))
X_WG = RigidTransform([0, 0, -0.05])
ground_geometry_id = plant.RegisterCollisionGeometry(
ground_box, RigidTransform(), Box(10, 10, 0.1), "ground",
CoulombFriction(0.9, 0.8))
plant.RegisterVisualGeometry(ground_box, RigidTransform(),
Box(10, 10,
0.1), "ground", [0.5, 0.5, 0.5, 1.])
plant.WeldFrames(plant.world_frame(), ground_box.body_frame(), X_WG)
# Add boxes
assert isinstance(masses, list)
assert isinstance(box_sizes, list)
assert len(masses) == len(box_sizes)
num_boxes = len(masses)
boxes = []
boxes_geometry_id = []
for i in range(num_boxes):
box_name = f"box{i}"
boxes.append(
plant.AddRigidBody(
box_name,
SpatialInertia(masses[i], np.array([0, 0, 0]),
UnitInertia(1, 1, 1))))
box_shape = Box(box_sizes[i][0], box_sizes[i][1], box_sizes[i][2])
boxes_geometry_id.append(
plant.RegisterCollisionGeometry(boxes[i], RigidTransform(),
box_shape, f"{box_name}_box",
CoulombFriction(0.9, 0.8)))
plant.RegisterVisualGeometry(ground_box, RigidTransform(), box_shape,
f"{box_name}_box", [0.5, 0.5, 0.5, 1.])
# Add small spheres at the corners of the box.
vertices = np.array([
[1, 1, 1], [1, 1, -1], [1, -1, 1], [1, -1, -1], [-1, 1, 1],
[-1, -1, 1], [-1, 1, -1], [-1, -1, -1]]) *\
np.tile(box_sizes[i]/2, (8, 1))
sphere_shape = Sphere(0.001)
for j in range(8):
plant.RegisterCollisionGeometry(boxes[i],
RigidTransform(vertices[j]),
sphere_shape,
f"{box_name}_sphere{j}",
CoulombFriction(0.9, 0.8))
plant.RegisterVisualGeometry(boxes[i], RigidTransform(vertices[j]),
sphere_shape, f"{box_name}_sphere{j}",
[0.5, 0.5, 0.5, 1])
plant.Finalize()
diagram = builder.Build()
return Environment(plant=plant,
scene_graph=scene_graph,
diagram=diagram,
boxes=boxes,
ground_geometry_id=ground_geometry_id,
boxes_geometry_id=boxes_geometry_id)
def load(self):
"""
Loads `meshcat` visualization elements.
@pre The `scene_graph` used to construct this object must be part of a
fully constructed diagram (e.g. via `DiagramBuilder.Build()`).
"""
self.vis[self.prefix].delete()
# Intercept load message via mock LCM.
mock_lcm = DrakeMockLcm()
DispatchLoadMessage(self._scene_graph, mock_lcm)
load_robot_msg = lcmt_viewer_load_robot.decode(
mock_lcm.get_last_published_message("DRAKE_VIEWER_LOAD_ROBOT"))
# Translate elements to `meshcat`.
for i in range(load_robot_msg.num_links):
link = load_robot_msg.link[i]
[source_name, frame_name] = link.name.split("::")
for j in range(link.num_geom):
geom = link.geom[j]
# MultibodyPlant currenly sets alpha=0 to make collision
# geometry "invisible". Ignore those geometries here.
if geom.color[3] == 0:
continue
element_local_tf = RigidTransform(
RotationMatrix(Quaternion(geom.quaternion)),
geom.position).GetAsMatrix4()
if geom.type == geom.BOX:
assert geom.num_float_data == 3
meshcat_geom = meshcat.geometry.Box(geom.float_data)
elif geom.type == geom.SPHERE:
assert geom.num_float_data == 1
meshcat_geom = meshcat.geometry.Sphere(geom.float_data[0])
elif geom.type == geom.CYLINDER:
assert geom.num_float_data == 2
meshcat_geom = meshcat.geometry.Cylinder(
geom.float_data[1], geom.float_data[0])
# In Drake, cylinders are along +z
# In meshcat, cylinders are along +y
# Rotate to fix this misalignment
extra_rotation = tf.rotation_matrix(
math.pi / 2., [1, 0, 0])
element_local_tf[0:3, 0:3] = \
element_local_tf[0:3, 0:3].dot(
extra_rotation[0:3, 0:3])
elif geom.type == geom.MESH:
meshcat_geom = \
meshcat.geometry.ObjMeshGeometry.from_file(
geom.string_data[0:-3] + "obj")
else:
print("UNSUPPORTED GEOMETRY TYPE {} IGNORED".format(
geom.type))
continue
# Turn a list of R,G,B elements (any indexable list of >= 3
# elements will work), where each element is specified on range
# [0., 1.], into the equivalent 24-bit value 0xRRGGBB.
def Rgb2Hex(rgb):
val = 0
for i in range(3):
val += (256**(2 - i)) * int(255 * rgb[i])
return val
self.vis[self.prefix][source_name][str(link.robot_num)][
frame_name][str(j)]\
.set_object(meshcat_geom,
meshcat.geometry
.MeshLambertMaterial(
color=Rgb2Hex(geom.color)))
self.vis[self.prefix][source_name][str(
link.robot_num)][frame_name][str(j)].set_transform(
element_local_tf)
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("--duration",
type=float,
default=np.inf,
help="Desired duration of the simulation in seconds.")
parser.add_argument(
"--hardware",
action='store_true',
help="Use the ManipulationStationHardwareInterface instead of an "
"in-process simulation.")
parser.add_argument("--test",
action='store_true',
help="Disable opening the gui window for testing.")
parser.add_argument(
"--filter_time_const",
type=float,
default=0.005,
help="Time constant for the first order low pass filter applied to"
"the teleop commands")
parser.add_argument(
"--velocity_limit_factor",
type=float,
default=1.0,
help="This value, typically between 0 and 1, further limits the "
"iiwa14 joint velocities. It multiplies each of the seven "
"pre-defined joint velocity limits. "
"Note: The pre-defined velocity limits are specified by "
"iiwa14_velocity_limits, found in this python file.")
parser.add_argument('--setup',
type=str,
default='manipulation_class',
help="The manipulation station setup to simulate. ",
choices=['manipulation_class', 'clutter_clearing'])
MeshcatVisualizer.add_argparse_argument(parser)
args = parser.parse_args()
if args.test:
# Don't grab mouse focus during testing.
grab_focus = False
# See: https://stackoverflow.com/a/52528832/7829525
os.environ["SDL_VIDEODRIVER"] = "dummy"
else:
grab_focus = True
builder = DiagramBuilder()
if args.hardware:
station = builder.AddSystem(ManipulationStationHardwareInterface())
station.Connect(wait_for_cameras=False)
else:
station = builder.AddSystem(ManipulationStation())
# Initializes the chosen station type.
if args.setup == 'manipulation_class':
station.SetupManipulationClassStation()
station.AddManipulandFromFile(
("drake/examples/manipulation_station/models/"
"061_foam_brick.sdf"),
RigidTransform(RotationMatrix.Identity(), [0.6, 0, 0]))
elif args.setup == 'clutter_clearing':
station.SetupClutterClearingStation()
ycb_objects = CreateDefaultYcbObjectList()
for model_file, X_WObject in ycb_objects:
station.AddManipulandFromFile(model_file, X_WObject)
station.Finalize()
ConnectDrakeVisualizer(builder, station.get_scene_graph(),
station.GetOutputPort("pose_bundle"))
if args.meshcat:
meshcat = builder.AddSystem(
MeshcatVisualizer(station.get_scene_graph(),
zmq_url=args.meshcat))
builder.Connect(station.GetOutputPort("pose_bundle"),
meshcat.get_input_port(0))
robot = station.get_controller_plant()
params = DifferentialInverseKinematicsParameters(robot.num_positions(),
robot.num_velocities())
time_step = 0.005
params.set_timestep(time_step)
# True velocity limits for the IIWA14 (in rad, rounded down to the first
# decimal)
iiwa14_velocity_limits = np.array([1.4, 1.4, 1.7, 1.3, 2.2, 2.3, 2.3])
# Stay within a small fraction of those limits for this teleop demo.
factor = args.velocity_limit_factor
params.set_joint_velocity_limits(
(-factor * iiwa14_velocity_limits, factor * iiwa14_velocity_limits))
differential_ik = builder.AddSystem(
DifferentialIK(robot, robot.GetFrameByName("iiwa_link_7"), params,
time_step))
builder.Connect(differential_ik.GetOutputPort("joint_position_desired"),
station.GetInputPort("iiwa_position"))
teleop = builder.AddSystem(MouseKeyboardTeleop(grab_focus=grab_focus))
filter_ = builder.AddSystem(
FirstOrderLowPassFilter(time_constant=args.filter_time_const, size=6))
builder.Connect(teleop.get_output_port(0), filter_.get_input_port(0))
builder.Connect(filter_.get_output_port(0),
differential_ik.GetInputPort("rpy_xyz_desired"))
builder.Connect(teleop.GetOutputPort("position"),
station.GetInputPort("wsg_position"))
builder.Connect(teleop.GetOutputPort("force_limit"),
station.GetInputPort("wsg_force_limit"))
diagram = builder.Build()
simulator = Simulator(diagram)
# This is important to avoid duplicate publishes to the hardware interface:
simulator.set_publish_every_time_step(False)
station_context = diagram.GetMutableSubsystemContext(
station, simulator.get_mutable_context())
station.GetInputPort("iiwa_feedforward_torque").FixValue(
station_context, np.zeros(7))
simulator.AdvanceTo(1e-6)
q0 = station.GetOutputPort("iiwa_position_measured").Eval(station_context)
differential_ik.parameters.set_nominal_joint_position(q0)
teleop.SetPose(differential_ik.ForwardKinematics(q0))
filter_.set_initial_output_value(
diagram.GetMutableSubsystemContext(filter_,
simulator.get_mutable_context()),
teleop.get_output_port(0).Eval(
diagram.GetMutableSubsystemContext(
teleop, simulator.get_mutable_context())))
differential_ik.SetPositions(
diagram.GetMutableSubsystemContext(differential_ik,
simulator.get_mutable_context()),
q0)
simulator.set_target_realtime_rate(args.target_realtime_rate)
print_instructions()
simulator.AdvanceTo(args.duration)
def test_render_engine_api(self):
class DummyRenderEngine(mut.render.RenderEngine):
"""Mirror of C++ DummyRenderEngine."""
# See comment below about `rgbd_sensor_test.cc`.
latest_instance = None
def __init__(self, render_label=None):
mut.render.RenderEngine.__init__(self)
# N.B. We do not hide these because this is a test class.
# Normally, you would want to hide this.
self.force_accept = False
self.registered_geometries = set()
self.updated_ids = {}
self.include_group_name = "in_test"
self.X_WC = RigidTransform_[float]()
self.color_count = 0
self.depth_count = 0
self.label_count = 0
self.color_camera = None
self.depth_camera = None
self.label_camera = None
def UpdateViewpoint(self, X_WC):
DummyRenderEngine.latest_instance = self
self.X_WC = X_WC
def ImplementGeometry(self, shape, user_data):
DummyRenderEngine.latest_instance = self
def DoRegisterVisual(self, id, shape, properties, X_WG):
DummyRenderEngine.latest_instance = self
mut.GetRenderLabelOrThrow(properties)
if self.force_accept or properties.HasGroup(
self.include_group_name
):
self.registered_geometries.add(id)
return True
return False
def DoUpdateVisualPose(self, id, X_WG):
DummyRenderEngine.latest_instance = self
self.updated_ids[id] = X_WG
def DoRemoveGeometry(self, id):
DummyRenderEngine.latest_instance = self
self.registered_geometries.remove(id)
def DoClone(self):
DummyRenderEngine.latest_instance = self
new = DummyRenderEngine()
new.force_accept = copy.copy(self.force_accept)
new.registered_geometries = copy.copy(
self.registered_geometries)
new.updated_ids = copy.copy(self.updated_ids)
new.include_group_name = copy.copy(self.include_group_name)
new.X_WC = copy.copy(self.X_WC)
new.color_count = copy.copy(self.color_count)
new.depth_count = copy.copy(self.depth_count)
new.label_count = copy.copy(self.label_count)
new.color_camera = copy.copy(self.color_camera)
new.depth_camera = copy.copy(self.depth_camera)
new.label_camera = copy.copy(self.label_camera)
return new
def DoRenderColorImage(self, camera, color_image_out):
DummyRenderEngine.latest_instance = self
self.color_count += 1
self.color_camera = camera
def DoRenderDepthImage(self, camera, depth_image_out):
DummyRenderEngine.latest_instance = self
self.depth_count += 1
self.depth_camera = camera
def DoRenderLabelImage(self, camera, label_image_out):
DummyRenderEngine.latest_instance = self
self.label_count += 1
self.label_camera = camera
engine = DummyRenderEngine()
self.assertIsInstance(engine, mut.render.RenderEngine)
self.assertIsInstance(engine.Clone(), DummyRenderEngine)
# Test implementation of C++ interface by using RgbdSensor.
renderer_name = "renderer"
builder = DiagramBuilder()
scene_graph = builder.AddSystem(mut.SceneGraph())
# N.B. This passes ownership.
scene_graph.AddRenderer(renderer_name, engine)
sensor = builder.AddSystem(RgbdSensor(
parent_id=scene_graph.world_frame_id(),
X_PB=RigidTransform(),
depth_camera=mut.render.DepthRenderCamera(
mut.render.RenderCameraCore(
renderer_name, CameraInfo(640, 480, np.pi/4),
mut.render.ClippingRange(0.1, 5.0), RigidTransform()),
mut.render.DepthRange(0.1, 5.0))))
builder.Connect(
scene_graph.get_query_output_port(),
sensor.query_object_input_port(),
)
diagram = builder.Build()
diagram_context = diagram.CreateDefaultContext()
sensor_context = sensor.GetMyContextFromRoot(diagram_context)
image = sensor.color_image_output_port().Eval(sensor_context)
# N.B. Because there's context cloning going on under the hood, we
# won't be interacting with our originally registered instance.
# See `rgbd_sensor_test.cc` as well.
current_engine = DummyRenderEngine.latest_instance
self.assertIsNot(current_engine, engine)
self.assertIsInstance(image, ImageRgba8U)
self.assertEqual(current_engine.color_count, 1)
image = sensor.depth_image_32F_output_port().Eval(sensor_context)
self.assertIsInstance(image, ImageDepth32F)
self.assertEqual(current_engine.depth_count, 1)
image = sensor.depth_image_16U_output_port().Eval(sensor_context)
self.assertIsInstance(image, ImageDepth16U)
self.assertEqual(current_engine.depth_count, 2)
image = sensor.label_image_output_port().Eval(sensor_context)
self.assertIsInstance(image, ImageLabel16I)
self.assertEqual(current_engine.label_count, 1)
# Confirm that the CameraProperties APIs are *not* available.
with catch_drake_warnings(expected_count=2):
cam = mut.render.CameraProperties(10, 10, np.pi / 4, "")
depth_cam = mut.render.DepthCameraProperties(10, 10, np.pi / 4, "",
0.1, 5)
with self.assertRaises(TypeError):
engine.RenderColorImage(
cam, True, ImageRgba8U(cam.width, cam.height))
with self.assertRaises(TypeError):
engine.RenderLabelImage(
cam, True, ImageLabel16I(cam.width, cam.height))
with self.assertRaises(TypeError):
engine.RenderDepthImage(
depth_cam, True,
ImageDepth32F(depth_cam.width, depth_cam.height))
def add_pusher(self):
"""
Adds a cylindrical pusher object
:return:
:rtype:
"""
mbp = self.mbp
parser = self.parser
pusher_model_idx = parser.AddModelFromFile(paths.xy_slide, "pusher")
base_link = mbp.GetBodyByName("base", pusher_model_idx)
# weld it to the world
mbp.WeldFrames(mbp.world_frame(), base_link.body_frame())
self.models['pusher'] = pusher_model_idx
radius = 0.01
length = 0.1
pusher_shape = Cylinder(radius, length)
# This rigid body will be added to the pusher model instance
world_body = mbp.world_body()
pusher_body = mbp.AddRigidBody(
"pusher_body", pusher_model_idx,
SpatialInertia(mass=10.0,
p_PScm_E=np.array([0., 0., 0.]),
G_SP_E=UnitInertia(1.0, 1.0, 1.0)))
self._rigid_bodies['pusher'] = pusher_body
# weld it to EE frame at particular height
translation = np.zeros(3)
translation[
2] = length / 2.0 + 0.001 # give it a little room for collision stuff
T_EE_P = RigidTransform(p=translation)
ee_link = mbp.GetBodyByName("end_effector", pusher_model_idx)
mbp.WeldFrames(ee_link.body_frame(), pusher_body.body_frame(), T_EE_P)
# color it green
color = np.array([0., 1., 0., 1.])
mbp.RegisterVisualGeometry(pusher_body, RigidTransform.Identity(),
pusher_shape, "pusher_vis", color)
mbp.RegisterCollisionGeometry(pusher_body, RigidTransform.Identity(),
pusher_shape, "pusher_collision",
CoulombFriction(0.9, 0.8))
def export_port_func():
# export relevant ports
actuation_input_port = mbp.get_actuation_input_port(
pusher_model_idx)
state_output_port = mbp.get_state_output_port(pusher_model_idx)
self._port_names["pusher_state_output"] = "pusher_state_output"
self._port_names[
"pusher_actuation_input"] = "pusher_actuation_input"
self.builder.ExportOutput(state_output_port,
self._port_names["pusher_state_output"])
self.builder.ExportInput(
actuation_input_port,
self._port_names["pusher_actuation_input"])
self._export_port_functions.append(export_port_func)
def __init__(self,
visible=Visible(),
min_range=MinRange(),
max_range=MaxRange(),
value=Value()):
"""
Args:
visible: An object with boolean elements for 'roll', 'pitch',
'yaw', 'x', 'y', 'z'; the intention is for this to be the
PoseSliders.Visible() namedtuple. Defaults to all true.
min_range, max_range, value: Objects with float values for 'roll',
'pitch', 'yaw', 'x', 'y', 'z'; the intention is for the
caller to use the PoseSliders.MinRange, MaxRange, and
Value namedtuples. See those tuples for default values.
"""
LeafSystem.__init__(self)
port = self.DeclareAbstractOutputPort(
"pose", lambda: AbstractValue.Make(RigidTransform()),
self.DoCalcOutput)
# The widgets themselves have undeclared state. For now, we accept it,
# and simply disable caching on the output port.
# TODO(russt): consider implementing the more elaborate methods seen
# in, e.g., LcmMessageSubscriber.
port.disable_caching_by_default()
# Note: This timing affects the keyboard teleop performance. A larger
# time step causes more lag in the response.
self.DeclarePeriodicEvent(0.01, 0.0,
PublishEvent(self._process_event_queue))
self._roll = FloatSlider(min=min_range.roll,
max=max_range.roll,
value=value.roll,
step=0.01,
continuous_update=True,
description="roll",
layout=Layout(width='90%'))
self._pitch = FloatSlider(min=min_range.pitch,
max=max_range.pitch,
value=value.pitch,
step=0.01,
continuous_update=True,
description="pitch",
layout=Layout(width='90%'))
self._yaw = FloatSlider(min=min_range.yaw,
max=max_range.yaw,
value=value.yaw,
step=0.01,
continuous_update=True,
description="yaw",
layout=Layout(width='90%'))
self._x = FloatSlider(min=min_range.x,
max=max_range.x,
value=value.x,
step=0.01,
continuous_update=True,
description="x",
orient='horizontal',
layout=Layout(width='90%'))
self._y = FloatSlider(min=min_range.y,
max=max_range.y,
value=value.y,
step=0.01,
continuous_update=True,
description="y",
layout=Layout(width='90%'))
self._z = FloatSlider(min=min_range.z,
max=max_range.z,
value=value.z,
step=0.01,
continuous_update=True,
description="z",
layout=Layout(width='90%'))
if visible.roll:
display(self._roll)
if visible.pitch:
display(self._pitch)
if visible.yaw:
display(self._yaw)
if visible.x:
display(self._x)
if visible.y:
display(self._y)
if visible.z:
display(self._z)
builder = DiagramBuilder()
if args.hardware:
# TODO(russt): Replace this hard-coded camera serial number with a config
# file.
camera_ids = ["805212060544"]
station = builder.AddSystem(
ManipulationStationHardwareInterface(camera_ids))
station.Connect(wait_for_cameras=False)
else:
station = builder.AddSystem(ManipulationStation())
station.SetupManipulationClassStation()
station.AddManipulandFromFile(
"drake/examples/manipulation_station/models/061_foam_brick.sdf",
RigidTransform(RotationMatrix.Identity(), [0.6, 0, 0]))
station.Finalize()
ConnectDrakeVisualizer(builder, station.get_scene_graph(),
station.GetOutputPort("pose_bundle"))
if args.meshcat:
meshcat = builder.AddSystem(
MeshcatVisualizer(station.get_scene_graph(),
zmq_url=args.meshcat,
open_browser=args.open_browser))
builder.Connect(station.GetOutputPort("pose_bundle"),
meshcat.get_input_port(0))
teleop = builder.AddSystem(
JointSliders(station.get_controller_plant(), length=800))
if args.test:
from pydrake.math import RigidTransform, RollPitchYaw
import utilities as utils
from scipy.special import erfinv
import pickle
import os
from tempfile import TemporaryFile
from pydrake.all import PiecewisePolynomial
from systems.visualization import Visualizer
# Create the block model with the default flat terrain
_file = "systems/urdf/sliding_block.urdf"
plant = TimeSteppingMultibodyPlant(file=_file)
body_inds = plant.multibody.GetBodyIndices(plant.model_index)
base_frame = plant.multibody.get_body(body_inds[0]).body_frame()
plant.multibody.WeldFrames(plant.multibody.world_frame(), base_frame,
RigidTransform())
plant.Finalize()
# Get the default context
context = plant.multibody.CreateDefaultContext()
# set chance constraints parameters
beta, theta, sigma = 0.6, 0.6, 0.3
chance_params = np.array([beta, theta, sigma])
# set friction ERM parameters
friction_variance = sigma
friction_bias = 0.01
friction_multiplier = 1e6
friction_erm_params = np.array(
[friction_variance, friction_bias, friction_multiplier])
# set normal distance ERM parameters
distance_variance = 0.1
def test_model_instance_state_access(self):
# Create a MultibodyPlant with a kuka arm and a schunk gripper.
# the arm is welded to the world, the gripper is welded to the
# arm's end effector.
wsg50_sdf_path = FindResourceOrThrow(
"drake/manipulation/models/" +
"wsg_50_description/sdf/schunk_wsg_50.sdf")
iiwa_sdf_path = FindResourceOrThrow(
"drake/manipulation/models/" +
"iiwa_description/sdf/iiwa14_no_collision.sdf")
plant = MultibodyPlant()
parser = Parser(plant)
iiwa_model = parser.AddModelFromFile(
file_name=iiwa_sdf_path, model_name='robot')
gripper_model = parser.AddModelFromFile(
file_name=wsg50_sdf_path, model_name='gripper')
# Weld the base of arm and gripper to reduce the number of states.
X_EeGripper = RigidTransform(
RollPitchYaw(np.pi / 2, 0, np.pi / 2), [0, 0, 0.081])
plant.WeldFrames(A=plant.world_frame(),
B=plant.GetFrameByName("iiwa_link_0", iiwa_model))
plant.WeldFrames(
A=plant.GetFrameByName("iiwa_link_7", iiwa_model),
B=plant.GetFrameByName("body", gripper_model),
X_AB=X_EeGripper)
plant.Finalize()
# Create a context of the MBP and set the state of the context
# to desired values.
context = plant.CreateDefaultContext()
nq = plant.num_positions()
nv = plant.num_velocities()
nq_iiwa = 7
nv_iiwa = 7
nq_gripper = 2
nv_gripper = 2
q_iiwa_desired = np.zeros(nq_iiwa)
v_iiwa_desired = np.zeros(nv_iiwa)
q_gripper_desired = np.zeros(nq_gripper)
v_gripper_desired = np.zeros(nv_gripper)
q_iiwa_desired[2] = np.pi/3
q_gripper_desired[0] = 0.1
v_iiwa_desired[1] = 5.0
q_gripper_desired[0] = -0.3
x_iiwa_desired = np.zeros(nq_iiwa + nv_iiwa)
x_iiwa_desired[0:nq_iiwa] = q_iiwa_desired
x_iiwa_desired[nq_iiwa:nq_iiwa+nv_iiwa] = v_iiwa_desired
x_gripper_desired = np.zeros(nq_gripper + nv_gripper)
x_gripper_desired[0:nq_gripper] = q_gripper_desired
x_gripper_desired[nq_gripper:nq_gripper+nv_gripper] = v_gripper_desired
x_desired = np.zeros(nq + nv)
x_desired[0:7] = q_iiwa_desired
x_desired[7:9] = q_gripper_desired
x_desired[nq:nq+7] = v_iiwa_desired
x_desired[nq+7:nq+nv] = v_gripper_desired
# Check SetPositionsAndVelocities() for each model instance.
# Do the iiwa model first.
plant.SetPositionsAndVelocities(context, np.zeros(nq + nv))
self.assertTrue(np.allclose(plant.GetPositionsAndVelocities(context),
np.zeros(nq + nv)))
plant.SetPositionsAndVelocities(context, iiwa_model, x_iiwa_desired)
self.assertTrue(np.allclose(
plant.GetPositionsAndVelocities(context, iiwa_model),
x_iiwa_desired))
self.assertTrue(np.allclose(plant.GetPositionsAndVelocities(
context, gripper_model), np.zeros(nq_gripper + nv_gripper)))
# Do the gripper model.
plant.SetPositionsAndVelocities(context, np.zeros(nq + nv))
self.assertTrue(np.allclose(plant.GetPositionsAndVelocities(context),
np.zeros(nq + nv)))
plant.SetPositionsAndVelocities(
context, gripper_model, x_gripper_desired)
self.assertTrue(np.allclose(
plant.GetPositionsAndVelocities(context, gripper_model),
x_gripper_desired))
self.assertTrue(np.allclose(plant.GetPositionsAndVelocities(
context, iiwa_model), np.zeros(nq_iiwa + nv_iiwa)))
# Check SetPositions() for each model instance.
# Do the iiwa model first.
plant.SetPositionsAndVelocities(context, np.zeros(nq + nv))
self.assertTrue(np.allclose(plant.GetPositionsAndVelocities(context),
np.zeros(nq + nv)))
plant.SetPositions(context, iiwa_model, q_iiwa_desired)
self.assertTrue(np.allclose(
plant.GetPositions(context, iiwa_model), q_iiwa_desired))
self.assertTrue(np.allclose(plant.GetVelocities(
context, iiwa_model), np.zeros(nv_iiwa)))
self.assertTrue(np.allclose(plant.GetPositionsAndVelocities(
context, gripper_model), np.zeros(nq_gripper + nv_gripper)))
# Do the gripper model.
plant.SetPositionsAndVelocities(context, np.zeros(nq + nv))
self.assertTrue(np.allclose(plant.GetPositionsAndVelocities(context),
np.zeros(nq + nv)))
plant.SetPositions(context, gripper_model, q_gripper_desired)
self.assertTrue(np.allclose(
plant.GetPositions(context, gripper_model),
q_gripper_desired))
self.assertTrue(np.allclose(plant.GetVelocities(
context, gripper_model), np.zeros(nq_gripper)))
self.assertTrue(np.allclose(plant.GetPositionsAndVelocities(
context, iiwa_model), np.zeros(nq_iiwa + nv_iiwa)))
# Check SetVelocities() for each model instance.
# Do the iiwa model first.
plant.SetPositionsAndVelocities(context, np.zeros(nq + nv))
self.assertTrue(np.allclose(plant.GetPositionsAndVelocities(context),
np.zeros(nq + nv)))
plant.SetVelocities(context, iiwa_model, v_iiwa_desired)
self.assertTrue(np.allclose(
plant.GetVelocities(context, iiwa_model), v_iiwa_desired))
self.assertTrue(np.allclose(plant.GetPositions(
context, iiwa_model), np.zeros(nq_iiwa)))
self.assertTrue(np.allclose(plant.GetPositionsAndVelocities(
context, gripper_model), np.zeros(nq_gripper + nv_gripper)))
# Do the gripper model.
plant.SetPositionsAndVelocities(context, np.zeros(nq + nv))
self.assertTrue(np.allclose(plant.GetPositionsAndVelocities(context),
np.zeros(nq + nv)))
plant.SetVelocities(context, gripper_model, v_gripper_desired)
self.assertTrue(np.allclose(
plant.GetVelocities(context, gripper_model),
v_gripper_desired))
self.assertTrue(np.allclose(plant.GetPositions(
context, gripper_model), np.zeros(nv_gripper)))
self.assertTrue(np.allclose(plant.GetPositionsAndVelocities(
context, iiwa_model), np.zeros(nq_iiwa + nv_iiwa)))
def _convert_mesh(geom):
# Given a LCM geometry message, forms a meshcat geometry and material
# for that geometry, as well as a local tf to the meshcat geometry
# that matches the LCM geometry.
# For MESH geometry, this function checks if a texture file exists next
# to the mesh file, and uses that to provide the material information if
# present. For BOX, SPHERE, CYLINDER geometry as well as MESH geometry
# not satisfying the above, this function uses the geom.color field to
# create a flat color for the object. In the case of other geometry types,
# both fields are returned as None.
meshcat_geom = None
material = None
element_local_tf = RigidTransform(
RotationMatrix(Quaternion(geom.quaternion)),
geom.position).GetAsMatrix4()
if geom.type == geom.BOX:
assert geom.num_float_data == 3
meshcat_geom = meshcat.geometry.Box(geom.float_data)
elif geom.type == geom.SPHERE:
assert geom.num_float_data == 1
meshcat_geom = meshcat.geometry.Sphere(geom.float_data[0])
elif geom.type == geom.CYLINDER:
assert geom.num_float_data == 2
meshcat_geom = meshcat.geometry.Cylinder(geom.float_data[1],
geom.float_data[0])
# In Drake, cylinders are along +z
# In meshcat, cylinders are along +y
# Rotate to fix this misalignment
extra_rotation = tf.rotation_matrix(math.pi / 2., [1, 0, 0])
element_local_tf[0:3, 0:3] = (element_local_tf[0:3, 0:3].dot(
extra_rotation[0:3, 0:3]))
elif geom.type == geom.MESH:
meshcat_geom = meshcat.geometry.ObjMeshGeometry.from_file(
geom.string_data[0:-3] + "obj")
# Handle scaling.
# TODO(gizatt): See meshcat-python#40 for incorporating scale as a
# field rather than a matrix multiplication.
scale = geom.float_data[:3]
element_local_tf[:3, :3] = element_local_tf[:3, :3].dot(np.diag(scale))
# Attempt to find a texture for the object by looking for an
# identically-named *.png next to the model.
# TODO(gizatt): Support .MTLs and prefer them over png, since they're
# both more expressive and more standard.
# TODO(gizatt): In the long term, this kind of material information
# should be gleaned from the SceneGraph constituents themselves, so
# that we visualize what the simulation is *actually* reasoning about
# rather than what files happen to be present.
candidate_texture_path_png = geom.string_data[0:-3] + "png"
if os.path.exists(candidate_texture_path_png):
material = meshcat.geometry.MeshLambertMaterial(
map=meshcat.geometry.ImageTexture(
image=meshcat.geometry.PngImage.from_file(
candidate_texture_path_png)))
else:
print("UNSUPPORTED GEOMETRY TYPE {} IGNORED".format(geom.type))
return meshcat_geom, material
if material is None:
def rgb_2_hex(rgb):
# Turn a list of R,G,B elements (any indexable list
# of >= 3 elements will work), where each element is
# specified on range [0., 1.], into the equivalent
# 24-bit value 0xRRGGBB.
val = 0
for i in range(3):
val += (256**(2 - i)) * int(255 * rgb[i])
return val
material = meshcat.geometry.MeshLambertMaterial(
color=rgb_2_hex(geom.color[:3]),
transparent=geom.color[3] != 1.,
opacity=geom.color[3])
return meshcat_geom, material, element_local_tf
def test_model_instance_state_access_by_array(self):
# Create a MultibodyPlant with a kuka arm and a schunk gripper.
# the arm is welded to the world, the gripper is welded to the
# arm's end effector.
wsg50_sdf_path = FindResourceOrThrow(
"drake/manipulation/models/" +
"wsg_50_description/sdf/schunk_wsg_50.sdf")
iiwa_sdf_path = FindResourceOrThrow(
"drake/manipulation/models/" +
"iiwa_description/sdf/iiwa14_no_collision.sdf")
timestep = 0.0002
plant = MultibodyPlant(timestep)
parser = Parser(plant)
iiwa_model = parser.AddModelFromFile(
file_name=iiwa_sdf_path, model_name='robot')
gripper_model = parser.AddModelFromFile(
file_name=wsg50_sdf_path, model_name='gripper')
# Weld the base of arm and gripper to reduce the number of states.
X_EeGripper = RigidTransform(
RollPitchYaw(np.pi / 2, 0, np.pi / 2), [0, 0, 0.081])
plant.WeldFrames(
A=plant.world_frame(),
B=plant.GetFrameByName("iiwa_link_0", iiwa_model))
plant.WeldFrames(
A=plant.GetFrameByName("iiwa_link_7", iiwa_model),
B=plant.GetFrameByName("body", gripper_model),
X_AB=X_EeGripper)
plant.Finalize()
# Create a context of the MBP and set the state of the context
# to desired values.
context = plant.CreateDefaultContext()
nq = plant.num_positions()
nq_iiwa = plant.num_positions(iiwa_model)
nv = plant.num_velocities()
nv_iiwa = plant.num_velocities(iiwa_model)
q_iiwa_desired = np.linspace(0, 0.3, 7)
v_iiwa_desired = q_iiwa_desired + 0.4
q_gripper_desired = [0.4, 0.5]
v_gripper_desired = [-1., -2.]
x_desired = np.zeros(nq + nv)
x_desired[0:7] = q_iiwa_desired
x_desired[7:9] = q_gripper_desired
x_desired[nq:nq+7] = v_iiwa_desired
x_desired[nq+7:nq+nv] = v_gripper_desired
x = plant.GetMutablePositionsAndVelocities(context=context)
x[:] = x_desired
q = plant.GetPositions(context=context)
v = plant.GetVelocities(context=context)
# Get state from context.
x = plant.GetPositionsAndVelocities(context=context)
x_tmp = plant.GetMutablePositionsAndVelocities(context=context)
self.assertTrue(np.allclose(x_desired, x_tmp))
# Get positions and velocities of specific model instances
# from the position/velocity vector of the plant.
q_iiwa = plant.GetPositions(context=context, model_instance=iiwa_model)
q_iiwa_array = plant.GetPositionsFromArray(
model_instance=iiwa_model, q=q)
self.assertTrue(np.allclose(q_iiwa, q_iiwa_array))
q_gripper = plant.GetPositions(
context=context, model_instance=gripper_model)
v_iiwa = plant.GetVelocities(
context=context, model_instance=iiwa_model)
v_iiwa_array = plant.GetVelocitiesFromArray(
model_instance=iiwa_model, v=v)
self.assertTrue(np.allclose(v_iiwa, v_iiwa_array))
v_gripper = plant.GetVelocities(
context=context, model_instance=gripper_model)
# Assert that the `GetPositions` and `GetVelocities` return
# the desired values set earlier.
self.assertTrue(np.allclose(q_iiwa_desired, q_iiwa))
self.assertTrue(np.allclose(v_iiwa_desired, v_iiwa))
self.assertTrue(np.allclose(q_gripper_desired, q_gripper))
self.assertTrue(np.allclose(v_gripper_desired, v_gripper))
# Verify that SetPositionsInArray() and SetVelocitiesInArray() works.
plant.SetPositionsInArray(
model_instance=iiwa_model, q_instance=np.zeros(nq_iiwa), q=q)
self.assertTrue(np.allclose(
plant.GetPositionsFromArray(model_instance=iiwa_model, q=q),
np.zeros(nq_iiwa)))
plant.SetVelocitiesInArray(
model_instance=iiwa_model, v_instance=np.zeros(nv_iiwa), v=v)
self.assertTrue(np.allclose(
plant.GetVelocitiesFromArray(model_instance=iiwa_model, v=v),
np.zeros(nv_iiwa)))
# Check actuation.
nu = plant.num_actuated_dofs()
u = np.zeros(nu)
u_iiwa = np.arange(nv_iiwa)
plant.SetActuationInArray(
model_instance=iiwa_model, u_instance=u_iiwa, u=u)
self.assertTrue(np.allclose(u[:7], u_iiwa))
def _xyz_rpy_deg(xyz, rpy_deg):
return RigidTransform(RollPitchYaw(np.asarray(rpy_deg) * np.pi / 180), xyz)
def DoPublish(self, context, event):
# TODO(SeanCurtis-TRI) We want to be able to use this visualizer to
# draw without having it part of a Simulator. That means we'd like
# vis.Publish(context) to work. Currently, pydrake offers no mechanism
# to declare a forced event. However, by overriding DoPublish and
# putting the forced event callback code in the override, we can
# simulate it.
# We need to bind a mechanism for declaring forced events so we don't
# have to resort to overriding the dispatcher.
LeafSystem.DoPublish(self, context, event)
contact_results = self.EvalAbstractInput(context, 0).get_value()
vis = self._meshcat_viz.vis[self._meshcat_viz.prefix]["contact_forces"]
contacts = []
for i_contact in range(contact_results.num_point_pair_contacts()):
contact_info = contact_results.point_pair_contact_info(i_contact)
# Do not display small forces.
force_norm = np.linalg.norm(contact_info.contact_force())
if force_norm < self._force_threshold:
continue
point_pair = contact_info.point_pair()
key = (point_pair.id_A.get_value(), point_pair.id_B.get_value())
cvis = vis[str(key)]
contacts.append(key)
arrow_height = self._radius*2.0
if key not in self._published_contacts:
# New key, so create the geometry. Note: the height of the
# cylinder is 2 and gets scaled to twice the contact force
# length, because I am drawing both (equal and opposite)
# forces. Note also that meshcat (following three.js) puts
# the height of the cylinder along the y axis.
cvis["cylinder"].set_object(meshcat.geometry.Cylinder(
height=2.0, radius=self._radius),
meshcat.geometry.MeshLambertMaterial(color=0x33cc33))
cvis["head"].set_object(meshcat.geometry.Cylinder(
height=arrow_height,
radiusTop=0, radiusBottom=self._radius*2.0),
meshcat.geometry.MeshLambertMaterial(color=0x00dd00))
cvis["tail"].set_object(meshcat.geometry.Cylinder(
height=arrow_height,
radiusTop=self._radius*2.0, radiusBottom=0),
meshcat.geometry.MeshLambertMaterial(color=0x00dd00))
height = force_norm/self._contact_force_scale
cvis["cylinder"].set_transform(tf.scale_matrix(
height, direction=[0, 1, 0]))
cvis["head"].set_transform(tf.translation_matrix(
[0, height + arrow_height/2.0, 0.0]))
cvis["tail"].set_transform(tf.translation_matrix(
[0, -height - arrow_height/2.0, 0.0]))
# The contact frame's origin is located at contact_point and is
# oriented such that Cy is aligned with the contact force.
p_WC = contact_info.contact_point() # documented as in world.
if force_norm < 1e-6:
# We cannot rely on self._force_threshold to determine if the
# force can be normalized; that threshold can be zero.
R_WC = RotationMatrix()
else:
fhat_C_W = contact_info.contact_force() / force_norm
R_WC = RotationMatrix.MakeFromOneVector(b_A=fhat_C_W,
axis_index=1)
X_WC = RigidTransform(R=R_WC, p=p_WC)
cvis.set_transform(X_WC.GetAsMatrix4())
# We only delete any contact vectors that did not persist into this
# publish. It is tempting to just delete() the root branch at the
# beginning of this publish, but this leads to visual artifacts
# (flickering) in the browser.
for key in set(self._published_contacts) - set(contacts):
vis[str(key)].delete()
self._published_contacts = contacts
def test_rgbd_sensor(self):
def check_ports(system):
self.assertIsInstance(system.query_object_input_port(), InputPort)
self.assertIsInstance(system.color_image_output_port(), OutputPort)
self.assertIsInstance(system.depth_image_32F_output_port(),
OutputPort)
self.assertIsInstance(system.depth_image_16U_output_port(),
OutputPort)
self.assertIsInstance(system.label_image_output_port(), OutputPort)
# Use HDTV size.
width = 1280
height = 720
color_properties = CameraProperties(width=width,
height=height,
fov_y=np.pi / 6,
renderer_name="renderer")
depth_properties = DepthCameraProperties(width=width,
height=height,
fov_y=np.pi / 6,
renderer_name="renderer",
z_near=0.1,
z_far=5.5)
# Put it at the origin.
X_WB = RigidTransform()
# This id would fail if we tried to render; no such id exists.
parent_id = FrameId.get_new_id()
camera_poses = mut.RgbdSensor.CameraPoses(X_BC=RigidTransform(),
X_BD=RigidTransform())
sensor = mut.RgbdSensor(parent_id=parent_id,
X_PB=X_WB,
color_properties=color_properties,
depth_properties=depth_properties,
camera_poses=camera_poses,
show_window=False)
def check_info(camera_info):
self.assertIsInstance(camera_info, mut.CameraInfo)
self.assertEqual(camera_info.width(), width)
self.assertEqual(camera_info.height(), height)
check_info(sensor.color_camera_info())
check_info(sensor.depth_camera_info())
self.assertIsInstance(sensor.X_BC(), RigidTransform)
self.assertIsInstance(sensor.X_BD(), RigidTransform)
self.assertEqual(sensor.parent_frame_id(), parent_id)
check_ports(sensor)
# Test discrete camera, reconstructing using single-properties
# constructor.
color_and_depth_properties = depth_properties
sensor = mut.RgbdSensor(parent_id=parent_id,
X_PB=X_WB,
properties=color_and_depth_properties,
camera_poses=camera_poses,
show_window=False)
period = mut.RgbdSensorDiscrete.kDefaultPeriod
discrete = mut.RgbdSensorDiscrete(sensor=sensor,
period=period,
render_label_image=True)
self.assertTrue(discrete.sensor() is sensor)
self.assertEqual(discrete.period(), period)
check_ports(discrete)
# That we can access the state as images.
context = discrete.CreateDefaultContext()
values = context.get_abstract_state()
self.assertIsInstance(values.get_value(0), Value[mut.ImageRgba8U])
self.assertIsInstance(values.get_value(1), Value[mut.ImageDepth32F])
self.assertIsInstance(values.get_value(2), Value[mut.ImageDepth16U])
self.assertIsInstance(values.get_value(3), Value[mut.ImageLabel16I])
def __init__(self,
scene_graph,
zmq_url="default",
draw_period=0.033333,
camera_tfs=[RigidTransform()],
material_overrides=[],
global_transform=RigidTransform(),
out_prefix=None,
show_figure=False):
"""
Args:
scene_graph: A SceneGraph object.
draw_period: The rate at which this class publishes rendered
images.
zmq_url: Optionally set a url to connect to the blender server.
Use zmp_url="default" to the value obtained by running a
single blender server in another terminal.
TODO(gizatt): Use zmp_url=None or zmq_url="new" to start a
new server (as a child of this process); a new web browser
will be opened (the url will also be printed to the console).
Use e.g. zmq_url="tcp://127.0.0.1:5556" to specify a
specific address.
camera tfs: List of RigidTransform camera tfs.
material_overrides: A list of tuples of regex rules and
material override arguments to be passed into a
a register material call, not including names. (Those are
assigned automatically by this class.)
global transform: RigidTransform that gets premultiplied to every object.
Note: This call will not return until it connects to the
Blender server.
"""
LeafSystem.__init__(self)
if out_prefix is not None:
self.out_prefix = out_prefix
else:
self.out_prefix = "/tmp/drake_blender_vis_"
self.current_publish_num = 0
self.set_name('blender_color_camera')
self.DeclarePeriodicPublish(draw_period, 0.0)
self.draw_period = draw_period
# Pose bundle (from SceneGraph) input port.
self.DeclareAbstractInputPort("lcm_visualization",
AbstractValue.Make(PoseBundle(0)))
# Optional pose bundle of bounding boxes.
self.DeclareAbstractInputPort("bounding_box_bundle",
AbstractValue.Make(BoundingBoxBundle(0)))
if zmq_url == "default":
zmq_url = "tcp://127.0.0.1:5556"
elif zmq_url == "new":
raise NotImplementedError("TODO")
if zmq_url is not None:
print("Connecting to blender server at zmq_url=" + zmq_url + "...")
self.bsi = BlenderServerInterface(zmq_url=zmq_url)
print("Connected to Blender server.")
self._scene_graph = scene_graph
# Compile regex for the material overrides
self.material_overrides = [(re.compile(x[0]), x[1])
for x in material_overrides]
self.global_transform = global_transform
self.camera_tfs = camera_tfs
def on_initialize(context, event):
self.load()
self.DeclareInitializationEvent(event=PublishEvent(
trigger_type=TriggerType.kInitialization, callback=on_initialize))
self.show_figure = show_figure
if self.show_figure:
plt.figure()