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)