def test_clutter_clearing_setup(self): station = ManipulationStation(time_step=0.001) station.SetupClutterClearingStation() num_station_bodies = ( station.get_multibody_plant().num_model_instances()) ycb_objects = CreateClutterClearingYcbObjectList() for model_file, X_WObject in ycb_objects: station.AddManipulandFromFile(model_file, X_WObject) station.Finalize() 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)) self.assertEqual(len(station.get_camera_names()), 1) self.assertEqual(station.get_multibody_plant().num_model_instances(), num_station_bodies + len(ycb_objects))
def test_manipulation_station(self): # Just check the spelling. station = ManipulationStation( time_step=0.001, collision_model=IiwaCollisionModel.kNoCollision) 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(q, context) np.testing.assert_array_equal(q, station.GetIiwaPosition(context)) station.SetIiwaVelocity(v, context) np.testing.assert_array_equal(v, station.GetIiwaVelocity(context)) q = 4.23 v = 8.51 station.SetWsgPosition(q, context) self.assertEqual(q, station.GetWsgPosition(context)) station.SetWsgVelocity(v, context) self.assertEqual(v, station.GetWsgVelocity(context)) station.get_camera_pose(0)
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_planar_iiwa_setup(self): station = ManipulationStation(time_step=0.001) station.SetupPlanarIiwaStation() station.Finalize() self.assertEqual(station.num_iiwa_joints(), 3) context = station.CreateDefaultContext() q = np.linspace(0.04, 0.6, num=3) v = np.linspace(-2.3, 0.5, num=3) 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))