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))
