def test_manipulation_station(self):
# Just check the spelling.
station = ManipulationStation(time_step=0.001)
station.SetupManipulationClassStation()
station.SetWsgGains(0.1, 0.1)
station.SetIiwaPositionGains(np.ones(7))
station.SetIiwaVelocityGains(np.ones(7))
station.SetIiwaIntegralGains(np.ones(7))
station.Finalize()
station.get_multibody_plant()
station.get_mutable_multibody_plant()
station.get_scene_graph()
station.get_mutable_scene_graph()
station.get_controller_plant()
# Check the setters/getters.
context = station.CreateDefaultContext()
q = np.linspace(0.04, 0.6, num=7)
v = np.linspace(-2.3, 0.5, num=7)
station.SetIiwaPosition(context, q)
np.testing.assert_array_equal(q, station.GetIiwaPosition(context))
station.SetIiwaVelocity(context, v)
np.testing.assert_array_equal(v, station.GetIiwaVelocity(context))
q = 0.0423
v = 0.0851
station.SetWsgPosition(context, q)
self.assertEqual(q, station.GetWsgPosition(context))
station.SetWsgVelocity(context, v)
self.assertEqual(v, station.GetWsgVelocity(context))
station.GetStaticCameraPosesInWorld()["0"]
self.assertEqual(len(station.get_camera_names()), 3)
def test_rgbd_sensor_registration_deprecated(self):
X_PC = RigidTransform(p=[1, 2, 3])
station = ManipulationStation(time_step=0.001)
station.SetupManipulationClassStation()
plant = station.get_multibody_plant()
with catch_drake_warnings(expected_count=2):
properties = DepthCameraProperties(10, 10, np.pi / 4, "renderer",
0.01, 5.0)
station.RegisterRgbdSensor("deprecated", plant.world_frame(), X_PC,
properties)
station.Finalize()
camera_poses = station.GetStaticCameraPosesInWorld()
# The three default cameras plus the one just added.
self.assertEqual(len(camera_poses), 4)
self.assertTrue("deprecated" in camera_poses)
def test_rgbd_sensor_registration(self):
X_PC = RigidTransform(p=[1, 2, 3])
station = ManipulationStation(time_step=0.001)
station.SetupManipulationClassStation()
plant = station.get_multibody_plant()
color_camera = ColorRenderCamera(
RenderCameraCore("renderer", CameraInfo(10, 10, np.pi / 4),
ClippingRange(0.1, 10.0), RigidTransform()),
False)
depth_camera = DepthRenderCamera(color_camera.core(),
DepthRange(0.1, 9.5))
station.RegisterRgbdSensor("single_sensor", plant.world_frame(), X_PC,
depth_camera)
station.RegisterRgbdSensor("dual_sensor", plant.world_frame(), X_PC,
color_camera, depth_camera)
station.Finalize()
camera_poses = station.GetStaticCameraPosesInWorld()
# The three default cameras plus the two just added.
self.assertEqual(len(camera_poses), 5)
self.assertTrue("single_sensor" in camera_poses)
self.assertTrue("dual_sensor" in camera_poses)
