Set camera poses to measured values.

RussTedrake · Oct 23, 2018 · ff98fe4 · ff98fe4
1 parent a3f913e
commit ff98fe4
Show file tree

Hide file tree

Showing 2 changed files with 18 additions and 4 deletions.
diff --git a/examples/manipulation_station/manipulation_station.cc b/examples/manipulation_station/manipulation_station.cc
@@ -472,10 +472,23 @@ template <typename T>
 Isometry3d ManipulationStation<T>::get_camera_pose(int camera_number) {
   DRAKE_DEMAND(camera_number >= 0 && camera_number <= 2);
 
-  // TODO(russt): Set these to the real camera poses.
-  Vector3<double> p_WC(-0.25, -0.75, 0.15);
-  Vector3<double> rpy_WC(0, 0, M_PI_2 * 0.9);
-
+  Vector3d p_WC;
+  Vector3d rpy_WC;
+
+  switch (camera_number) {
+    case 0:
+      p_WC << -0.233066, -0.451461, 0.466761;
+      rpy_WC << 1.69101, 0.176488, 0.432721;
+      break;
+    case 1:
+      p_WC << -0.197236, 0.468471, 0.436499;
+      rpy_WC << -1.68974, 0.20245, -0.706783;
+      break;
+    case 2:
+      p_WC << 0.786905, -0.0284378, 1.04287;
+      rpy_WC << 0.0438918, 1.03776, -3.13612;
+      break;
+  }
   return RigidTransform<double>(RollPitchYaw<double>(rpy_WC), p_WC)
       .GetAsIsometry3();
 }

diff --git a/examples/manipulation_station/proof_of_life.cc b/examples/manipulation_station/proof_of_life.cc
@@ -35,6 +35,7 @@ int do_main(int argc, char* argv[]) {
 
   // Create the "manipulation station".
   auto station = builder.AddSystem<ManipulationStation>();
+  station->AddCupboard();
   station->Finalize();
 
   geometry::ConnectDrakeVisualizer(&builder, station->get_mutable_scene_graph(),