def SetPose(self, pose):
"""
@param pose is an Isometry3.
"""
tf = RigidTransform(pose)
self.SetRPY(RollPitchYaw(tf.rotation()))
self.SetXYZ(pose.translation())
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'])
parser.add_argument(
"-w",
"--open-window",
dest="browser_new",
action="store_const",
const=1,
default=None,
help="Open the MeshCat display in a new browser window.")
args = parser.parse_args()
builder = DiagramBuilder()
# NOTE: the meshcat instance is always created in order to create the
# teleop controls (joint sliders and open/close gripper button). When
# args.hardware is True, the meshcat server will *not* display robot
# geometry, but it will contain the joint sliders and open/close gripper
# button in the "Open Controls" tab in the top-right of the viewing server.
meshcat = Meshcat()
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()
geometry_query_port = station.GetOutputPort("geometry_query")
DrakeVisualizer.AddToBuilder(builder, geometry_query_port)
meshcat_visualizer = MeshcatVisualizerCpp.AddToBuilder(
builder=builder,
query_object_port=geometry_query_port,
meshcat=meshcat)
if args.setup == 'planar':
meshcat.Set2dRenderMode()
pyplot_visualizer = ConnectPlanarSceneGraphVisualizer(
builder, station.get_scene_graph(), geometry_query_port)
if args.browser_new is not None:
url = meshcat.web_url()
webbrowser.open(url=url, new=args.browser_new)
teleop = builder.AddSystem(
JointSliders(meshcat=meshcat, plant=station.get_controller_plant()))
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(meshcat=meshcat))
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(VectorLogSink(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.SetPositions(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:
iiwa_velocities_log = iiwa_velocities.FindLog(simulator.get_context())
for time, qdot in zip(iiwa_velocities_log.sample_times(),
iiwa_velocities_log.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 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 test_optimization(self):
"""Tests geometry::optimization bindings"""
A = np.eye(3)
b = [1.0, 1.0, 1.0]
prog = MathematicalProgram()
x = prog.NewContinuousVariables(3, "x")
t = prog.NewContinuousVariables(1, "t")
# Test Point.
p = np.array([11.1, 12.2, 13.3])
point = mut.optimization.Point(p)
self.assertEqual(point.ambient_dimension(), 3)
np.testing.assert_array_equal(point.x(), p)
point.set_x(x=2*p)
np.testing.assert_array_equal(point.x(), 2*p)
point.set_x(x=p)
# Test HPolyhedron.
hpoly = mut.optimization.HPolyhedron(A=A, b=b)
self.assertEqual(hpoly.ambient_dimension(), 3)
np.testing.assert_array_equal(hpoly.A(), A)
np.testing.assert_array_equal(hpoly.b(), b)
self.assertTrue(hpoly.PointInSet(x=[0, 0, 0], tol=0.0))
hpoly.AddPointInSetConstraints(prog, x)
with self.assertRaisesRegex(
RuntimeError, ".*not implemented yet for HPolyhedron.*"):
hpoly.ToShapeWithPose()
h_box = mut.optimization.HPolyhedron.MakeBox(
lb=[-1, -1, -1], ub=[1, 1, 1])
h_unit_box = mut.optimization.HPolyhedron.MakeUnitBox(dim=3)
np.testing.assert_array_equal(h_box.A(), h_unit_box.A())
np.testing.assert_array_equal(h_box.b(), h_unit_box.b())
self.assertIsInstance(
h_box.MaximumVolumeInscribedEllipsoid(),
mut.optimization.Hyperellipsoid)
np.testing.assert_array_almost_equal(
h_box.ChebyshevCenter(), [0, 0, 0])
# Test Hyperellipsoid.
ellipsoid = mut.optimization.Hyperellipsoid(A=A, center=b)
self.assertEqual(ellipsoid.ambient_dimension(), 3)
np.testing.assert_array_equal(ellipsoid.A(), A)
np.testing.assert_array_equal(ellipsoid.center(), b)
self.assertTrue(ellipsoid.PointInSet(x=b, tol=0.0))
ellipsoid.AddPointInSetConstraints(prog, x)
shape, pose = ellipsoid.ToShapeWithPose()
self.assertIsInstance(shape, mut.Ellipsoid)
self.assertIsInstance(pose, RigidTransform)
scale, witness = ellipsoid.MinimumUniformScalingToTouch(point)
self.assertTrue(scale > 0.0)
np.testing.assert_array_almost_equal(witness, p)
e_ball = mut.optimization.Hyperellipsoid.MakeAxisAligned(
radius=[1, 1, 1], center=b)
np.testing.assert_array_equal(e_ball.A(), A)
np.testing.assert_array_equal(e_ball.center(), b)
e_ball2 = mut.optimization.Hyperellipsoid.MakeHypersphere(
radius=1, center=b)
np.testing.assert_array_equal(e_ball2.A(), A)
np.testing.assert_array_equal(e_ball2.center(), b)
e_ball3 = mut.optimization.Hyperellipsoid.MakeUnitBall(dim=3)
np.testing.assert_array_equal(e_ball3.A(), A)
np.testing.assert_array_equal(e_ball3.center(), [0, 0, 0])
# Test VPolytope.
vertices = np.array([[0.0, 1.0, 2.0], [3.0, 7.0, 5.0]])
vpoly = mut.optimization.VPolytope(vertices=vertices)
self.assertEqual(vpoly.ambient_dimension(), 2)
np.testing.assert_array_equal(vpoly.vertices(), vertices)
self.assertTrue(vpoly.PointInSet(x=[1.0, 5.0], tol=1e-8))
vpoly.AddPointInSetConstraints(prog, x[0:2])
v_box = mut.optimization.VPolytope.MakeBox(
lb=[-1, -1, -1], ub=[1, 1, 1])
self.assertTrue(v_box.PointInSet([0, 0, 0]))
v_unit_box = mut.optimization.VPolytope.MakeUnitBox(dim=3)
self.assertTrue(v_unit_box.PointInSet([0, 0, 0]))
# Test remaining ConvexSet methods using these instances.
self.assertIsInstance(hpoly.Clone(), mut.optimization.HPolyhedron)
self.assertTrue(ellipsoid.IsBounded())
hpoly.AddPointInNonnegativeScalingConstraints(prog=prog, x=x, t=t[0])
# Test MakeFromSceneGraph methods.
scene_graph = mut.SceneGraph()
source_id = scene_graph.RegisterSource("source")
frame_id = scene_graph.RegisterFrame(
source_id=source_id, frame=mut.GeometryFrame("frame"))
box_geometry_id = scene_graph.RegisterGeometry(
source_id=source_id, frame_id=frame_id,
geometry=mut.GeometryInstance(X_PG=RigidTransform(),
shape=mut.Box(1., 1., 1.),
name="sphere"))
sphere_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"))
context = scene_graph.CreateDefaultContext()
pose_vector = mut.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.optimization.HPolyhedron(
query_object=query_object, geometry_id=box_geometry_id,
reference_frame=scene_graph.world_frame_id())
self.assertEqual(H.ambient_dimension(), 3)
E = mut.optimization.Hyperellipsoid(
query_object=query_object, geometry_id=sphere_geometry_id,
reference_frame=scene_graph.world_frame_id())
self.assertEqual(E.ambient_dimension(), 3)
P = mut.optimization.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.optimization.VPolytope(
query_object=query_object, geometry_id=box_geometry_id,
reference_frame=scene_graph.world_frame_id())
self.assertEqual(V.ambient_dimension(), 3)
# Test Iris.
obstacles = mut.optimization.MakeIrisObstacles(
query_object=query_object,
reference_frame=scene_graph.world_frame_id())
options = mut.optimization.IrisOptions()
options.require_sample_point_is_contained = True
options.iteration_limit = 1
options.termination_threshold = 0.1
region = mut.optimization.Iris(
obstacles=obstacles, sample=[2, 3.4, 5],
domain=mut.optimization.HPolyhedron.MakeBox(
lb=[-5, -5, -5], ub=[5, 5, 5]), options=options)
self.assertIsInstance(region, mut.optimization.HPolyhedron)
obstacles = [
mut.optimization.HPolyhedron.MakeUnitBox(3),
mut.optimization.Hyperellipsoid.MakeUnitBall(3),
mut.optimization.Point([0, 0, 0]),
mut.optimization.VPolytope.MakeUnitBox(3)]
region = mut.optimization.Iris(
obstacles=obstacles, sample=[2, 3.4, 5],
domain=mut.optimization.HPolyhedron.MakeBox(
lb=[-5, -5, -5], ub=[5, 5, 5]), options=options)
self.assertIsInstance(region, mut.optimization.HPolyhedron)
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)
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())
# 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)
def _xyz_rpy_deg(xyz, rpy_deg):
return RigidTransform(RollPitchYaw(np.asarray(rpy_deg) * np.pi / 180), xyz)
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 = {}
mock_lcm = DrakeMockLcm()
mock_lcm_subscriber = Subscriber(lcm=mock_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, 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
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() is not ".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_exploding_iiwa_sim(self):
"""
Shows a simulation of a falling + exploding IIWA which "changes
topology" by being rebuilt without joints.
"""
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder,
time_step=1e-3)
iiwa_file = FindResourceOrThrow(
"drake/manipulation/models/iiwa_description/urdf/"
"iiwa14_spheres_dense_elbow_collision.urdf")
Parser(plant).AddModelFromFile(iiwa_file, "iiwa")
# Add ground plane.
X_FH = HalfSpace.MakePose([0, 0, 1], [0, 0, 0])
plant.RegisterCollisionGeometry(plant.world_body(), X_FH, HalfSpace(),
"ground_plane_collision",
CoulombFriction(0.8, 0.3))
plant.Finalize()
# Loosey-goosey - no control.
for model in me.get_model_instances(plant):
util.build_with_no_control(builder, plant, model)
if VISUALIZE:
print("test_exploding_iiwa_sim")
role = Role.kIllustration
DrakeVisualizer.AddToBuilder(
builder, scene_graph, params=DrakeVisualizerParams(role=role))
ConnectContactResultsToDrakeVisualizer(builder, plant, scene_graph)
diagram = builder.Build()
# Set up context.
d_context = diagram.CreateDefaultContext()
context = plant.GetMyContextFromRoot(d_context)
# - Hoist IIWA up in the air.
plant.SetFreeBodyPose(context, plant.GetBodyByName("base"),
RigidTransform([0, 0, 1.]))
# - Set joint velocities to "spin" it in the air.
for joint in me.get_joints(plant):
if isinstance(joint, RevoluteJoint):
me.set_joint_positions(plant, context, joint, 0.7)
me.set_joint_velocities(plant, context, joint, -5.)
def monitor(d_context):
# Stop the simulation once there's any contact.
context = plant.GetMyContextFromRoot(d_context)
query_object = plant.get_geometry_query_input_port().Eval(context)
if query_object.HasCollisions():
return EventStatus.ReachedTermination(plant, "Contact")
else:
return EventStatus.DidNothing()
# Forward simulate.
simulator = Simulator(diagram, d_context)
simulator.Initialize()
simulator.set_monitor(monitor)
if VISUALIZE:
simulator.set_target_realtime_rate(1.)
simulator.AdvanceTo(2.)
# Try to push a bit further.
simulator.clear_monitor()
simulator.AdvanceTo(d_context.get_time() + 0.05)
diagram.Publish(d_context)
# Recreate simulator.
builder_new = DiagramBuilder()
plant_new, scene_graph_new = AddMultibodyPlantSceneGraph(
builder_new, time_step=plant.time_step())
subgraph = mut.MultibodyPlantSubgraph(
mut.get_elements_from_plant(plant, scene_graph))
# Remove all joints; make them floating bodies.
for joint in me.get_joints(plant):
subgraph.remove_joint(joint)
# Remove massless bodies.
# For more info, see: https://stackoverflow.com/a/62035705/7829525
for body in me.get_bodies(plant):
if body is plant.world_body():
continue
if body.default_mass() == 0.:
subgraph.remove_body(body)
# Finalize.
to_new = subgraph.add_to(plant_new, scene_graph_new)
plant_new.Finalize()
if VISUALIZE:
DrakeVisualizer.AddToBuilder(
builder_new,
scene_graph_new,
params=DrakeVisualizerParams(role=role))
ConnectContactResultsToDrakeVisualizer(builder_new, plant_new,
scene_graph_new)
diagram_new = builder_new.Build()
# Remap state.
d_context_new = diagram_new.CreateDefaultContext()
d_context_new.SetTime(d_context.get_time())
context_new = plant_new.GetMyContextFromRoot(d_context_new)
to_new.copy_state(context, context_new)
# Simulate.
simulator_new = Simulator(diagram_new, d_context_new)
simulator_new.Initialize()
diagram_new.Publish(d_context_new)
if VISUALIZE:
simulator_new.set_target_realtime_rate(1.)
simulator_new.AdvanceTo(context_new.get_time() + 2)
if VISUALIZE:
input(" Press enter...")
def main():
goal_position = np.array([0.5, 0., 0.025])
blue_box_clean_position = [0.4, 0., 0.05]
red_box_clean_position = [0.6, 0., 0.05]
goal_delta = 0.05
parser = argparse.ArgumentParser(description=__doc__)
MeshcatVisualizer.add_argparse_argument(parser)
parser.add_argument('--use_meshcat',
action='store_true',
help="Must be set for meshcat to be used.")
parser.add_argument('--disable_planar_viz',
action='store_true',
help="Don't create a planar visualizer. Probably"
" breaks something that assumes the planar"
" vis exists.")
parser.add_argument('--teleop',
action='store_true',
help="Enable teleop, so *don't* use the state machine"
" and motion primitives.")
args = parser.parse_args()
builder = DiagramBuilder()
# Set up the ManipulationStation
station = builder.AddSystem(ManipulationStation(0.001))
mbp = station.get_multibody_plant()
station.SetupManipulationClassStation()
add_goal_region_visual_geometry(mbp, goal_position, goal_delta)
add_box_at_location(mbp,
name="blue_box",
color=[0.25, 0.25, 1., 1.],
pose=RigidTransform(p=[0.4, 0.0, 0.025]))
add_box_at_location(mbp,
name="red_box",
color=[1., 0.25, 0.25, 1.],
pose=RigidTransform(p=[0.6, 0.0, 0.025]))
station.Finalize()
iiwa_q0 = np.array([0.0, 0.6, 0.0, -1.75, 0., 1., np.pi / 2.])
# Attach a visualizer.
if args.use_meshcat:
meshcat = builder.AddSystem(
MeshcatVisualizer(station.get_scene_graph(), zmq_url=args.meshcat))
builder.Connect(station.GetOutputPort("pose_bundle"),
meshcat.get_input_port(0))
if not args.disable_planar_viz:
plt.gca().clear()
viz = builder.AddSystem(
PlanarSceneGraphVisualizer(station.get_scene_graph(),
xlim=[0.25, 0.8],
ylim=[-0.1, 0.5],
ax=plt.gca()))
builder.Connect(station.GetOutputPort("pose_bundle"),
viz.get_input_port(0))
plt.draw()
# Hook up DifferentialIK, since both control modes use it.
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 = 1.0
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))
differential_ik.set_name("Differential IK")
differential_ik.parameters.set_nominal_joint_position(iiwa_q0)
builder.Connect(differential_ik.GetOutputPort("joint_position_desired"),
station.GetInputPort("iiwa_position"))
if not args.teleop:
symbol_list = [
SymbolL2Close("blue_box_in_goal", "blue_box", goal_position,
goal_delta),
SymbolL2Close("red_box_in_goal", "red_box", goal_position,
goal_delta),
SymbolRelativePositionL2("blue_box_on_red_box",
"blue_box",
"red_box",
l2_thresh=0.01,
offset=np.array([0., 0., 0.05])),
SymbolRelativePositionL2("red_box_on_blue_box",
"red_box",
"blue_box",
l2_thresh=0.01,
offset=np.array([0., 0., 0.05])),
]
primitive_list = [
MoveBoxPrimitive("put_blue_box_in_goal", mbp, "blue_box",
goal_position),
MoveBoxPrimitive("put_red_box_in_goal", mbp, "red_box",
goal_position),
MoveBoxPrimitive("put_blue_box_away", mbp, "blue_box",
blue_box_clean_position),
MoveBoxPrimitive("put_red_box_away", mbp, "red_box",
red_box_clean_position),
MoveBoxPrimitive("put_red_box_on_blue_box", mbp, "red_box",
np.array([0., 0., 0.05]), "blue_box"),
MoveBoxPrimitive("put_blue_box_on_red_box", mbp, "blue_box",
np.array([0., 0., 0.05]), "red_box"),
]
task_execution_system = builder.AddSystem(
TaskExectionSystem(
mbp,
symbol_list=symbol_list,
primitive_list=primitive_list,
dfa_json_file="specifications/red_and_blue_boxes_stacking.json"
))
builder.Connect(station.GetOutputPort("plant_continuous_state"),
task_execution_system.GetInputPort("mbp_state_vector"))
builder.Connect(task_execution_system.get_output_port(0),
differential_ik.GetInputPort("rpy_xyz_desired"))
builder.Connect(task_execution_system.get_output_port(1),
station.GetInputPort("wsg_position"))
#movebox = MoveBoxPrimitive("test_move_box", mbp, "red_box", goal_position)
#rpy_xyz_trajectory, gripper_traj = movebox.generate_rpyxyz_and_gripper_trajectory(mbp.CreateDefaultContext())
#rpy_xyz_trajectory_source = builder.AddSystem(TrajectorySource(rpy_xyz_trajectory))
#builder.Connect(rpy_xyz_trajectory_source.get_output_port(0),
# differential_ik.GetInputPort("rpy_xyz_desired"))
#wsg_position_source = builder.AddSystem(TrajectorySource(gripper_traj))
#builder.Connect(wsg_position_source.get_output_port(0),
# station.GetInputPort("wsg_position"))
# Target zero feedforward residual torque at all times.
fft = builder.AddSystem(ConstantVectorSource(np.zeros(7)))
builder.Connect(fft.get_output_port(0),
station.GetInputPort("iiwa_feedforward_torque"))
input_force_fix = builder.AddSystem(ConstantVectorSource([40.0]))
builder.Connect(input_force_fix.get_output_port(0),
station.GetInputPort("wsg_force_limit"))
end_time = 10000
else: # Set up teleoperation.
# Hook up a pygame-based keyboard+mouse interface for
# teleoperation, and low pass its output to drive the EE target
# for the differential IK.
print_instructions()
teleop = builder.AddSystem(MouseKeyboardTeleop(grab_focus=True))
filter_ = builder.AddSystem(
FirstOrderLowPassFilter(time_constant=0.005, 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"))
# Target zero feedforward residual torque at all times.
fft = builder.AddSystem(ConstantVectorSource(np.zeros(7)))
builder.Connect(fft.get_output_port(0),
station.GetInputPort("iiwa_feedforward_torque"))
# Simulate functionally forever.
end_time = 10000
# Create symbol log
#symbol_log = SymbolFromTransformLog(
# [SymbolL2Close('at_goal', 'red_box', goal_position, .025),
# SymbolL2Close('at_goal', 'blue_box', goal_position, .025)])
#
#symbol_logger_system = builder.AddSystem(
# SymbolLoggerSystem(
# station.get_multibody_plant(), symbol_logger=symbol_log))
#builder.Connect(
# station.GetOutputPort("plant_continuous_state"),
# symbol_logger_system.GetInputPort("mbp_state_vector"))
# Remaining input ports need to be tied up.
diagram = builder.Build()
g = pydot.graph_from_dot_data(diagram.GetGraphvizString())[0]
g.write_png("system_diagram.png")
diagram_context = diagram.CreateDefaultContext()
station_context = diagram.GetMutableSubsystemContext(
station, diagram_context)
station.SetIiwaPosition(station_context, iiwa_q0)
differential_ik.SetPositions(
diagram.GetMutableSubsystemContext(differential_ik, diagram_context),
iiwa_q0)
if args.teleop:
teleop.SetPose(differential_ik.ForwardKinematics(iiwa_q0))
filter_.set_initial_output_value(
diagram.GetMutableSubsystemContext(filter_, diagram_context),
teleop.get_output_port(0).Eval(
diagram.GetMutableSubsystemContext(teleop, diagram_context)))
simulator = Simulator(diagram, diagram_context)
simulator.set_publish_every_time_step(False)
simulator.set_target_realtime_rate(1.0)
simulator.AdvanceTo(end_time)
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
# 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_deprecated(parent_id, X_PB):
# One deprecation warning for CameraPoses and one for RgbdSensor.
with catch_drake_warnings(expected_count=4):
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)
camera_poses = mut.RgbdSensor.CameraPoses(
X_BC=RigidTransform(), X_BD=RigidTransform())
return mut.RgbdSensor(parent_id=parent_id,
X_PB=X_PB,
color_properties=color_properties,
depth_properties=depth_properties,
camera_poses=camera_poses,
show_window=False)
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_deprecated(parent_id, X_PB):
with catch_drake_warnings(expected_count=3):
depth_properties = DepthCameraProperties(
width=width,
height=height,
fov_y=np.pi / 6,
renderer_name="renderer",
z_near=0.1,
z_far=5.5)
return mut.RgbdSensor(
parent_id=parent_id,
X_PB=X_PB,
properties=depth_properties,
camera_poses=mut.RgbdSensor.CameraPoses(),
show_window=False)
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_deprecated, construct, construct_single_deprecated,
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 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 plant, scene_graph, diagram, boxes, ground_geometry_id,\
boxes_geometry_id
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.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 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(
"--time_step", type=float, default=0.005,
help="Time constant for the differential IK solver and 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'])
parser.add_argument(
'--schunk_collision_model', type=str, default='box',
help="The Schunk collision model to use for simulation. ",
choices=['box', 'box_plus_fingertip_spheres'])
MeshcatVisualizer.add_argparse_argument(parser)
args = parser.parse_args()
if args.test:
# Don't grab mouse focus during testing.
# See: https://stackoverflow.com/a/52528832/7829525
os.environ["SDL_VIDEODRIVER"] = "dummy"
builder = DiagramBuilder()
if args.hardware:
station = builder.AddSystem(ManipulationStationHardwareInterface())
station.Connect(wait_for_cameras=False)
else:
station = builder.AddSystem(ManipulationStation())
if args.schunk_collision_model == "box":
schunk_model = SchunkCollisionModel.kBox
elif args.schunk_collision_model == "box_plus_fingertip_spheres":
schunk_model = SchunkCollisionModel.kBoxPlusFingertipSpheres
# Initializes the chosen station type.
if args.setup == 'manipulation_class':
station.SetupManipulationClassStation(
schunk_model=schunk_model)
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(
schunk_model=schunk_model)
ycb_objects = CreateClutterClearingYcbObjectList()
for model_file, X_WObject in ycb_objects:
station.AddManipulandFromFile(model_file, X_WObject)
station.Finalize()
DrakeVisualizer.AddToBuilder(builder,
station.GetOutputPort("query_object"))
if args.meshcat:
meshcat = ConnectMeshcatVisualizer(
builder, output_port=station.GetOutputPort("geometry_query"),
zmq_url=args.meshcat, open_browser=args.open_browser)
if args.setup == 'planar':
meshcat.set_planar_viewpoint()
robot = station.get_controller_plant()
params = DifferentialInverseKinematicsParameters(robot.num_positions(),
robot.num_velocities())
params.set_timestep(args.time_step)
# True velocity limits for the IIWA14 (in rad/s, 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, args.time_step))
builder.Connect(differential_ik.GetOutputPort("joint_position_desired"),
station.GetInputPort("iiwa_position"))
teleop = builder.AddSystem(DualShock4Teleop(initialize_joystick()))
filter_ = builder.AddSystem(
FirstOrderLowPassFilter(time_constant=args.time_step, 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))
# 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)
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 plot_mathematical_program(meshcat,
path,
prog,
X,
Y,
result=None,
point_size=0.05):
assert prog.num_vars() == 2
assert X.size == Y.size
N = X.size
values = np.vstack((X.reshape(-1), Y.reshape(-1)))
costs = prog.GetAllCosts()
# Vectorized multiply for the quadratic form.
# Z = (D*np.matmul(Q,D)).sum(0).reshape(nx, ny)
if costs:
Z = prog.EvalBindingVectorized(costs[0], values)
for b in costs[1:]:
Z = Z + prog.EvalBindingVectorized(b, values)
cv = f"{path}/constraints"
for binding in prog.GetAllConstraints():
if isinstance(binding.evaluator(), BoundingBoxConstraint):
c = binding.evaluator()
var_indices = [
int(prog.decision_variable_index()[v.get_id()])
for v in binding.variables()
]
satisfied = np.array(
c.CheckSatisfiedVectorized(values[var_indices, :],
0.001)).reshape(1, -1)
if costs:
Z[~satisfied] = np.nan
v = f"{cv}/{type(c).__name__}"
Zc = np.zeros(Z.shape)
Zc[satisfied] = np.nan
plot_surface(meshcat,
v,
X,
Y,
Zc.reshape(X.shape),
rgba=Rgba(1.0, .2, .2, 1.0),
wireframe=True)
else:
Zc = prog.EvalBindingVectorized(binding, values)
evaluator = binding.evaluator()
low = evaluator.lower_bound()
up = evaluator.upper_bound()
cvb = f"{cv}/{type(evaluator).__name__}"
for index in range(Zc.shape[0]):
# TODO(russt): Plot infeasible points in a different color.
infeasible = np.logical_or(Zc[index, :] < low[index],
Zc[index, :] > up[index])
plot_surface(meshcat,
f"{cvb}/{index}",
X,
Y,
Zc[index, :].reshape(X.shape),
rgba=Rgba(1.0, .3, 1.0, 1.0),
wireframe=True)
if costs:
plot_surface(meshcat,
f"{path}/objective",
X,
Y,
Z.reshape(X.shape),
rgba=Rgba(.3, 1.0, .3, 1.0),
wireframe=True)
if result:
v = f"{path}/solution"
meshcat.SetObject(v, Sphere(point_size), Rgba(.3, 1.0, .3, 1.0))
x_solution = result.get_x_val()
meshcat.SetTransform(
v,
RigidTransform(
[x_solution[0], x_solution[1],
result.get_optimal_cost()]))
def _get_transform(self):
return RigidTransform(
RollPitchYaw(self._value[0], self._value[1], self._value[2]),
self._value[3:])
def test_composition_gripper_workflow(self):
"""Tests subgraphs (pre-finalize) for composition, with a scene graph,
welding bodies together across different subgraphs."""
# N.B. The frame-welding is done so that we can easily set the
# positions of the IIWA / WSG without having to worry about / work
# around the floating body coordinates.
# Create IIWA.
iiwa_builder = DiagramBuilder()
iiwa_plant, iiwa_scene_graph = AddMultibodyPlantSceneGraph(
iiwa_builder, time_step=0.)
iiwa_file = FindResourceOrThrow(
"drake/manipulation/models/iiwa_description/urdf/"
"iiwa14_spheres_dense_elbow_collision.urdf")
iiwa_model = Parser(iiwa_plant).AddModelFromFile(iiwa_file, "iiwa")
iiwa_plant.WeldFrames(iiwa_plant.world_frame(),
iiwa_plant.GetFrameByName("base"))
iiwa_plant.Finalize()
if VISUALIZE:
print("test_composition")
DrakeVisualizer.AddToBuilder(iiwa_builder, iiwa_scene_graph)
iiwa_diagram = iiwa_builder.Build()
iiwa_subgraph = mut.MultibodyPlantSubgraph(
mut.get_elements_from_plant(iiwa_plant, iiwa_scene_graph))
self.assertIsInstance(iiwa_subgraph, mut.MultibodyPlantSubgraph)
iiwa_subgraph.remove_body(iiwa_plant.world_body())
# Create WSG.
wsg_builder = DiagramBuilder()
wsg_plant, wsg_scene_graph = AddMultibodyPlantSceneGraph(wsg_builder,
time_step=0.)
wsg_file = FindResourceOrThrow(
"drake/manipulation/models/wsg_50_description/sdf/"
"schunk_wsg_50.sdf")
wsg_model = Parser(wsg_plant).AddModelFromFile(wsg_file,
"gripper_model")
wsg_plant.WeldFrames(wsg_plant.world_frame(),
wsg_plant.GetFrameByName("__model__"))
wsg_plant.Finalize()
if VISUALIZE:
DrakeVisualizer.AddToBuilder(wsg_builder, wsg_scene_graph)
wsg_diagram = wsg_builder.Build()
wsg_subgraph = mut.MultibodyPlantSubgraph(
mut.get_elements_from_plant(wsg_plant, wsg_scene_graph))
self.assertIsInstance(wsg_subgraph, mut.MultibodyPlantSubgraph)
wsg_subgraph.remove_body(wsg_plant.world_body())
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder,
time_step=1e-3)
iiwa_to_plant = iiwa_subgraph.add_to(plant, scene_graph)
self.assertIsInstance(iiwa_to_plant, mut.MultibodyPlantElementsMap)
wsg_to_plant = wsg_subgraph.add_to(plant, scene_graph)
self.assertIsInstance(wsg_to_plant, mut.MultibodyPlantElementsMap)
if VISUALIZE:
DrakeVisualizer.AddToBuilder(builder, scene_graph)
rpy_deg = np.array([90., 0., 90])
X_7G = RigidTransform(p=[0, 0, 0.114],
rpy=RollPitchYaw(rpy_deg * np.pi / 180))
frame_7 = plant.GetFrameByName("iiwa_link_7")
frame_G = plant.GetFrameByName("body")
plant.WeldFrames(frame_7, frame_G, X_7G)
plant.Finalize()
q_iiwa = [0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7]
q_wsg = [-0.03, 0.03]
iiwa_d_context = iiwa_diagram.CreateDefaultContext()
iiwa_context = iiwa_plant.GetMyContextFromRoot(iiwa_d_context)
iiwa_plant.SetPositions(iiwa_context, iiwa_model, q_iiwa)
wsg_d_context = wsg_diagram.CreateDefaultContext()
wsg_context = wsg_plant.GetMyContextFromRoot(wsg_d_context)
wsg_plant.SetPositions(wsg_context, wsg_model, q_wsg)
# Transfer state and briefly compare.
diagram = builder.Build()
d_context = diagram.CreateDefaultContext()
context = plant.GetMyContextFromRoot(d_context)
iiwa_to_plant.copy_state(iiwa_context, context)
wsg_to_plant.copy_state(wsg_context, context)
# Compare frames from sub-plants.
util.compare_frame_poses(plant, context, iiwa_plant, iiwa_context,
"base", "iiwa_link_7")
util.compare_frame_poses(plant, context, wsg_plant, wsg_context,
"body", "left_finger")
# Visualize.
if VISUALIZE:
print(" Visualize IIWA")
Simulator(iiwa_diagram, iiwa_d_context.Clone()).Initialize()
input(" Press enter...")
print(" Visualize WSG")
Simulator(wsg_diagram, wsg_d_context.Clone()).Initialize()
input(" Press enter...")
print(" Visualize Composite")
Simulator(diagram, d_context.Clone()).Initialize()
input(" Press enter...")
def _write_pose_msg(X_AB, p, q):
# p - position message
# q - quaternion message
X_AB = RigidTransform(X_AB)
p.x, p.y, p.z = X_AB.translation()
q.w, q.x, q.y, q.z = X_AB.rotation().ToQuaternion().wxyz()
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 _read_pose_msg(p, q):
# p - position message
# q - quaternion message
return RigidTransform(Quaternion(wxyz=[q.w, q.x, q.y, q.z]),
[p.x, p.y, p.z])
def __init__(self,
scene_graph,
draw_period=1. / 30,
T_VW=np.array([[1., 0., 0., 0.], [0., 0., 1., 0.],
[0., 0., 0., 1.]]),
xlim=[-1., 1],
ylim=[-1, 1],
facecolor=[1, 1, 1],
use_random_colors=False,
substitute_collocated_mesh_files=True,
ax=None,
show=None):
"""
Args:
scene_graph: A SceneGraph object.
draw_period: The rate at which this class publishes to the
visualizer.
T_VW: The view projection matrix from world to view coordinates.
xlim: View limit into the scene.
ylim: View limit into the scene.
facecolor: Passed through to figure() and sets background color.
Both color name strings and RGB triplets are allowed. Defaults
to white.
use_random_colors: If set to True, will render each body with a
different color. (Multiple visual elements on the same body
will be the same color.)
substitute_collocated_mesh_files: If True, then a mesh file
specified with an unsupported filename extension may be
replaced by a file of the same base name in the same directory,
but with a supported filename extension. Currently only .obj
files are supported.
ax: If supplied, the visualizer will draw onto those axes instead
of creating a new set of axes. The visualizer will still change
the view range and figure size of those axes.
show: Opens a window during initialization / publish iff True.
Default is None, which implies show=True unless
matplotlib.get_backend() is 'template'.
"""
default_size = matplotlib.rcParams['figure.figsize']
scalefactor = (ylim[1] - ylim[0]) / (xlim[1] - xlim[0])
figsize = (default_size[0], default_size[0] * scalefactor)
PyPlotVisualizer.__init__(self,
facecolor=facecolor,
figsize=figsize,
ax=ax,
draw_period=draw_period,
show=show)
self.set_name('planar_multibody_visualizer')
self._scene_graph = scene_graph
self._T_VW = T_VW
# Pose bundle (from SceneGraph) input port.
# TODO(tehbelinda): Rename the `lcm_visualization` port to match
# SceneGraph once its output port has been updated. See #12214.
self._pose_bundle_port = self.DeclareAbstractInputPort(
"lcm_visualization", AbstractValue.Make(PoseBundle(0)))
self.ax.axis('equal')
self.ax.axis('off')
# Achieve the desired view limits.
self.ax.set_xlim(xlim)
self.ax.set_ylim(ylim)
default_size = self.fig.get_size_inches()
self.fig.set_size_inches(figsize[0], figsize[1])
# Populate body patches.
self._build_body_patches(use_random_colors,
substitute_collocated_mesh_files)
# Populate the body fill list -- which requires doing most of a draw
# pass, but with an ax.fill() command to initialize the draw patches.
# After initialization, we can then use in-place replacement of vertex
# positions. The body fill list stores the ax patch objects in the
# order they were spawned (i.e. by body, and then by order of view_
# patches). Drawing the tree should update them by iterating over
# bodies and patches in the same order.
self._body_fill_dict = {}
X_WB_initial = RigidTransform.Identity()
for full_name in self._patch_Blist.keys():
patch_Wlist, view_colors = self._get_view_patches(
full_name, X_WB_initial)
self._body_fill_dict[full_name] = []
for patch_W, color in zip(patch_Wlist, view_colors):
# Project the full patch the first time, to initialize a vertex
# list with enough space for any possible convex hull of this
# vertex set.
patch_V = self._project_patch(patch_W)
body_fill = self.ax.fill(patch_V[0, :],
patch_V[1, :],
zorder=0,
edgecolor='k',
facecolor=color,
closed=True)[0]
self._body_fill_dict[full_name].append(body_fill)
# Then update the vertices for a more accurate initial draw.
self._update_body_fill_verts(body_fill, patch_V)
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.1,
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', 'planar'])
parser.add_argument(
'--schunk_collision_model', type=str, default='box',
help="The Schunk collision model to use for simulation. ",
choices=['box', 'box_plus_fingertip_spheres'])
MeshcatVisualizer.add_argparse_argument(parser)
args = parser.parse_args()
builder = DiagramBuilder()
if args.hardware:
station = builder.AddSystem(ManipulationStationHardwareInterface())
station.Connect(wait_for_cameras=False)
else:
station = builder.AddSystem(ManipulationStation())
if args.schunk_collision_model == "box":
schunk_model = SchunkCollisionModel.kBox
elif args.schunk_collision_model == "box_plus_fingertip_spheres":
schunk_model = SchunkCollisionModel.kBoxPlusFingertipSpheres
# Initializes the chosen station type.
if args.setup == 'manipulation_class':
station.SetupManipulationClassStation(
schunk_model=schunk_model)
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(
schunk_model=schunk_model)
ycb_objects = CreateClutterClearingYcbObjectList()
for model_file, X_WObject in ycb_objects:
station.AddManipulandFromFile(model_file, X_WObject)
elif args.setup == 'planar':
station.SetupPlanarIiwaStation(
schunk_model=schunk_model)
station.AddManipulandFromFile(
"drake/examples/manipulation_station/models/"
+ "061_foam_brick.sdf",
RigidTransform(RotationMatrix.Identity(), [0.6, 0, 0]))
station.Finalize()
# If using meshcat, don't render the cameras, since RgbdCamera
# rendering only works with drake-visualizer. Without this check,
# running this code in a docker container produces libGL errors.
if args.meshcat:
meshcat = ConnectMeshcatVisualizer(
builder, output_port=station.GetOutputPort("geometry_query"),
zmq_url=args.meshcat, open_browser=args.open_browser)
if args.setup == 'planar':
meshcat.set_planar_viewpoint()
elif args.setup == 'planar':
pyplot_visualizer = builder.AddSystem(PlanarSceneGraphVisualizer(
station.get_scene_graph()))
builder.Connect(station.GetOutputPort("pose_bundle"),
pyplot_visualizer.get_input_port(0))
else:
DrakeVisualizer.AddToBuilder(builder,
station.GetOutputPort("query_object"))
image_to_lcm_image_array = builder.AddSystem(
ImageToLcmImageArrayT())
image_to_lcm_image_array.set_name("converter")
for name in station.get_camera_names():
cam_port = (
image_to_lcm_image_array
.DeclareImageInputPort[PixelType.kRgba8U](
"camera_" + name))
builder.Connect(
station.GetOutputPort("camera_" + name + "_rgb_image"),
cam_port)
image_array_lcm_publisher = builder.AddSystem(
LcmPublisherSystem.Make(
channel="DRAKE_RGBD_CAMERA_IMAGES",
lcm_type=image_array_t,
lcm=None,
publish_period=0.1,
use_cpp_serializer=True))
image_array_lcm_publisher.set_name("rgbd_publisher")
builder.Connect(
image_to_lcm_image_array.image_array_t_msg_output_port(),
image_array_lcm_publisher.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])
if args.setup == 'planar':
# Extract the 3 joints that are not welded in the planar version.
iiwa14_velocity_limits = iiwa14_velocity_limits[1:6:2]
# The below constant is in body frame.
params.set_end_effector_velocity_gain([1, 0, 0, 0, 1, 1])
# 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(EndEffectorTeleop(args.setup == 'planar'))
if args.test:
teleop.window.withdraw() # Don't display the window when testing.
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"))
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.
num_iiwa_joints = station.num_iiwa_joints()
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)
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)
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 __init__(self, config=None):
if config is None:
config_path = os.path.join(os.path.dirname(__file__),
"config.yaml")
config = yaml.safe_load(open(config_path, 'r'))
self._config = config
self._step_dt = config["step_dt"]
self._model_name = config["model_name"]
self._sim_diagram = DrakeSimDiagram(config)
mbp = self._sim_diagram.mbp
builder = self._sim_diagram.builder
# === Add table =====
dims = config["table"]["size"]
color = np.array(config["table"]["color"])
friction_params = config["table"]["coulomb_friction"]
box_shape = Box(*dims)
X_W_T = RigidTransform(p=np.array([0., 0., -dims[2] / 2.]))
# This rigid body will be added to the world model instance since
# the model instance is not specified.
box_body = mbp.AddRigidBody(
"table",
SpatialInertia(mass=1.0,
p_PScm_E=np.array([0., 0., 0.]),
G_SP_E=UnitInertia(1.0, 1.0, 1.0)))
mbp.WeldFrames(mbp.world_frame(), box_body.body_frame(), X_W_T)
mbp.RegisterVisualGeometry(box_body, RigidTransform(), box_shape,
"table_vis", color)
mbp.RegisterCollisionGeometry(box_body, RigidTransform(), box_shape,
"table_collision",
CoulombFriction(*friction_params))
# === Add pusher ====
parser = Parser(mbp, self._sim_diagram.sg)
self._pusher_model_id = parser.AddModelFromFile(
path_util.pusher_peg, "pusher")
base_link = mbp.GetBodyByName("base", self._pusher_model_id)
mbp.WeldFrames(mbp.world_frame(), base_link.body_frame())
def pusher_port_func():
actuation_input_port = mbp.get_actuation_input_port(
self._pusher_model_id)
state_output_port = mbp.get_state_output_port(
self._pusher_model_id)
builder.ExportInput(actuation_input_port, "pusher_actuation_input")
builder.ExportOutput(state_output_port, "pusher_state_output")
self._sim_diagram.finalize_functions.append(pusher_port_func)
# === Add slider ====
parser = Parser(mbp, self._sim_diagram.sg)
self._slider_model_id = parser.AddModelFromFile(
path_util.model_paths[self._model_name], self._model_name)
def slider_port_func():
state_output_port = mbp.get_state_output_port(
self._slider_model_id)
builder.ExportOutput(state_output_port, "slider_state_output")
self._sim_diagram.finalize_functions.append(slider_port_func)
if "rgbd_sensors" in config and config["rgbd_sensors"]["enabled"]:
self._sim_diagram.add_rgbd_sensors_from_config(config)
if "visualization" in config:
if not config["visualization"]:
pass
elif config["visualization"] == "meshcat":
self._sim_diagram.connect_to_meshcat()
elif config["visualization"] == "drake_viz":
self._sim_diagram.connect_to_drake_visualizer()
else:
raise ValueError("Unknown visualization:",
config["visualization"])
self._sim_diagram.finalize()
builder = DiagramBuilder()
builder.AddSystem(self._sim_diagram)
pid = builder.AddSystem(
PidController(kp=[0, 0], kd=[1000, 1000], ki=[0, 0]))
builder.Connect(self._sim_diagram.GetOutputPort("pusher_state_output"),
pid.get_input_port_estimated_state())
builder.Connect(
pid.get_output_port_control(),
self._sim_diagram.GetInputPort("pusher_actuation_input"))
builder.ExportInput(pid.get_input_port_desired_state(),
"pid_input_port_desired_state")
self._diagram = builder.Build()
self._pid_desired_state_port = self._diagram.get_input_port(0)
self._simulator = Simulator(self._diagram)
self.reset()
self.action_space = spaces.Box(low=-1.,
high=1.,
shape=(2, ),
dtype=np.float32)
# TODO: Observation space for images is currently too loose
self.observation_space = convert_observation_to_space(
self.get_observation())
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 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)
def xyz_rpy_deg(xyz, rpy_deg):
"""Shorthand for defining a pose."""
rpy_deg = np.asarray(rpy_deg)
return RigidTransform(RollPitchYaw(rpy_deg * np.pi / 180), xyz)
def test_pose_aggregator(self):
aggregator = PoseAggregator()
# - Set-up.
instance_id1 = 5 # Supply a random instance id.
port1 = aggregator.AddSingleInput("pose_only", instance_id1)
self.assertEqual(port1.get_data_type(), PortDataType.kVectorValued)
self.assertEqual(port1.size(), PoseVector.kSize)
instance_id2 = 42 # Supply another random, but unique, id.
ports2 = aggregator.AddSinglePoseAndVelocityInput(
"pose_and_velocity", instance_id2)
self.assertEqual(ports2.pose_input_port.get_data_type(),
PortDataType.kVectorValued)
self.assertEqual(ports2.pose_input_port.size(), PoseVector.kSize)
self.assertEqual(ports2.velocity_input_port.get_data_type(),
PortDataType.kVectorValued)
self.assertEqual(ports2.velocity_input_port.size(),
FrameVelocity.kSize)
num_poses = 1
port3 = aggregator.AddBundleInput("pose_bundle", num_poses)
self.assertEqual(port3.get_data_type(), PortDataType.kAbstractValued)
# - CalcOutput.
self.assertEqual(
aggregator.get_output_port(0).get_data_type(),
PortDataType.kAbstractValued)
context = aggregator.CreateDefaultContext()
output = aggregator.AllocateOutput()
p1 = [0, 1, 2]
pose1 = PoseVector()
pose1.set_translation(p1)
p2 = [5, 7, 9]
pose2 = PoseVector()
pose2.set_translation(p2)
w = [0.3, 0.4, 0.5]
v = [0.5, 0.6, 0.7]
velocity = FrameVelocity()
velocity.set_velocity(SpatialVelocity(w=w, v=v))
p3 = [50, 70, 90]
q3 = Quaternion(wxyz=normalized([0.1, 0.3, 0.7, 0.9]))
bundle = PoseBundle(num_poses)
bundle.set_transform(0, RigidTransform(quaternion=q3, p=p3))
bundle_value = AbstractValue.Make(bundle)
aggregator.get_input_port(0).FixValue(context, pose1)
aggregator.get_input_port(1).FixValue(context, pose2)
aggregator.get_input_port(2).FixValue(context, velocity)
aggregator.get_input_port(3).FixValue(context, bundle_value)
aggregator.CalcOutput(context, output)
value = output.get_data(0).get_value()
self.assertEqual(value.get_num_poses(), 3)
pose1_actual = RigidTransform(p=p1)
self.assertTrue((value.get_transform(0).GetAsMatrix34() ==
pose1_actual.GetAsMatrix34()).all())
pose2_actual = RigidTransform(p=p2)
self.assertTrue((value.get_transform(1).GetAsMatrix34() ==
pose2_actual.GetAsMatrix34()).all())
vel_actual = value.get_velocity(1).get_velocity()
self.assertTrue(np.allclose(vel_actual.rotational(), w))
self.assertTrue(np.allclose(vel_actual.translational(), v))
pose3_actual = RigidTransform(quaternion=q3, p=p3)
self.assertTrue((value.get_transform(2).GetAsMatrix34() ==
pose3_actual.GetAsMatrix34()).all())
def test_multibody_tree_kinematics(self):
file_name = FindResourceOrThrow(
"drake/examples/double_pendulum/models/double_pendulum.sdf")
plant = MultibodyPlant()
Parser(plant).AddModelFromFile(file_name)
plant.Finalize()
context = plant.CreateDefaultContext()
world_frame = plant.world_frame()
base = plant.GetBodyByName("base")
base_frame = plant.GetFrameByName("base")
X_WL = plant.CalcRelativeTransform(context,
frame_A=world_frame,
frame_B=base_frame)
self.assertIsInstance(X_WL, RigidTransform)
p_AQi = plant.CalcPointsPositions(context=context,
frame_B=base_frame,
p_BQi=np.array([[0, 1, 2],
[10, 11, 12]]).T,
frame_A=world_frame).T
self.assertTupleEqual(p_AQi.shape, (2, 3))
Jv_WL = plant.CalcFrameGeometricJacobianExpressedInWorld(
context=context, frame_B=base_frame, p_BoFo_B=[0, 0, 0])
self.assertTupleEqual(Jv_WL.shape, (6, plant.num_velocities()))
nq = plant.num_positions()
nv = plant.num_velocities()
wrt_list = [
(JacobianWrtVariable.kQDot, nq),
(JacobianWrtVariable.kV, nv),
]
for wrt, nw in wrt_list:
Jw_ABp_E = plant.CalcJacobianSpatialVelocity(context=context,
with_respect_to=wrt,
frame_B=base_frame,
p_BP=np.zeros(3),
frame_A=world_frame,
frame_E=world_frame)
self.assert_sane(Jw_ABp_E)
self.assertEqual(Jw_ABp_E.shape, (6, nw))
# Compute body pose.
X_WBase = plant.EvalBodyPoseInWorld(context, base)
self.assertIsInstance(X_WBase, RigidTransform)
# Set pose for the base.
X_WB_desired = RigidTransform.Identity()
X_WB = plant.CalcRelativeTransform(context, world_frame, base_frame)
plant.SetFreeBodyPose(context=context, body=base, X_WB=X_WB_desired)
self.assertTrue(np.allclose(X_WB.matrix(), X_WB_desired.matrix()))
# Set a spatial velocity for the base.
v_WB = SpatialVelocity(w=[1, 2, 3], v=[4, 5, 6])
plant.SetFreeBodySpatialVelocity(context=context, body=base, V_WB=v_WB)
v_base = plant.EvalBodySpatialVelocityInWorld(context, base)
self.assertTrue(np.allclose(v_base.rotational(), v_WB.rotational()))
self.assertTrue(
np.allclose(v_base.translational(), v_WB.translational()))
# Compute accelerations.
vdot = np.zeros(nv)
A_WB_array = plant.CalcSpatialAccelerationsFromVdot(context=context,
known_vdot=vdot)
self.assertEqual(len(A_WB_array), plant.num_bodies())
def setUp(self):
self.points = np.array([[1, 1, 0], [2, 1, 0]]).T
self.translation = RigidTransform(p=[1, 2, 3])
self.rotation = RigidTransform(
RotationMatrix(RollPitchYaw(0, 0, np.pi / 2)))
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 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_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))
