RobotLocomotion · jmcculloch2018 · Oct 17, 2018 · Nov 5, 2018 · Nov 5, 2018 · Nov 6, 2018
diff --git a/.gitmodules b/.gitmodules
@@ -53,6 +53,7 @@
 [submodule "src/catkin_projects/wsg50-ros-pkg"]
 	path = src/catkin_projects/wsg50-ros-pkg
 	url = [email protected]:RobotLocomotion/wsg50-ros-pkg
+	branch = pf-fix-ascii
 [submodule "src/catkin_projects/razer_hydra_ros"]
 	path = src/catkin_projects/razer_hydra_ros
-	url = [email protected]:aleeper/razer_hydra_ros
+	url = [email protected]:aleeper/razer_hydra
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -209,7 +209,7 @@ ExternalProject_Add(spartan_lcmtypes
 
 if (WITH_BULLET3)
   set(BULLET3_GIT_REPOSITORY
-  "git@github.com:bulletphysics/bullet3.git"
+  "https://github.com/bulletphysics/bullet3.git"
   CACHE STRING
   "Location of bullet3 repo.")
 
@@ -268,7 +268,7 @@ if (WITH_TRIMESH)
       message(FATAL_ERROR "Not using linux, don't know how to install v-hacd.")
     endif()
 
-    SET(TRIMESH_GIT_REPOSITORY git@github.com:mikedh/trimesh.git)
+    SET(TRIMESH_GIT_REPOSITORY https://github.com/gizatt/trimesh.git)
     SET(TRIMESH_GIT_TAG 2.35.15)
     # Installs our customized version of trimesh
     # to the project's site-packages build folder.

diff --git a/apps/iiwa/iiwa_hardware.pmd b/apps/iiwa/iiwa_hardware.pmd
@@ -132,6 +132,11 @@ group "8.ROS" {
     host = "localhost";
   }
 
+  cmd "2.1 Static Transform Publisher"{
+    exec = "roslaunch station_config static_transforms.launch";
+    host = "localhost";
+  }
+
   cmd "3.rviz" {
     exec = "rosrun rviz rviz -d $SPARTAN_SOURCE_DIR/src/catkin_projects/station_config/RLG_iiwa_1/rviz.rviz";
     host = "localhost";
@@ -167,8 +172,12 @@ group "8.ROS" {
     host = "localhost";
   }
 
+
+
 }
 
+
+
 group "9.iiwa_sim" {
 
   cmd "0.drake_iiwa_sim" {
@@ -186,11 +195,33 @@ group "9.iiwa_sim" {
     host = "localhost";
   }
 
-  cmd "3.ROS-OpenNI-drake-iiwa-sim" {
-    exec = "roslaunch camera_config openni2_drake_iiwa_sim.launch camera_serial_number:=carmine_1";
+  cmd "3.ROS-OpenNI-drake-iiwa-sim-left" {
+    exec = "roslaunch camera_config openni2_drake_iiwa_sim.launch camera_serial_number:=sim_d415_left";
     host = "localhost";
   }
 
+  cmd "4.ROS-OpenNI-drake-iiwa-sim-right" {
+    exec = "roslaunch camera_config openni2_drake_iiwa_sim.launch camera_serial_number:=sim_d415_right";
+    host = "localhost";
+  }
+
+}
+
+
+group "imitation" {
+    cmd "1.imitation_tools" {
+        exec = "roslaunch imitation_tools imitation_bagging.launch camera_serial_number:=sim_d415_left";
+        host = "localhost";
+    }
+    cmd "2.vive_teleop" {
+        exec = "python $SPARTAN_SOURCE_DIR/scripts/bin/vive_teleop.py";
+        host = "localhost";
+    }
+}
+
+cmd "imitation_extract" {
+    exec = "python ./src/catkin_projects/imitation_tools/scripts/batch_extract_all_logs.py && cp -r ./data_volume ./export";
+    host = "localhost";
 }
 
 script "1.1.robot_full_startup" {
@@ -276,3 +307,23 @@ script "6.start_drake_iiwa_sim"
   start cmd "2.state-translator";
   start cmd "4.director";
 }
+
+script "7.start_vive_imitation"
+{
+  start cmd "0.roscore";
+  wait ms 3000;
+
+  start cmd "2.ROS Model and TF Publisher";
+  start cmd "3.rviz";
+  start cmd "4.Robot Movement Service";
+  start cmd "5.IK Service";
+  start cmd "8.fusion_server";
+
+  start group "9.iiwa_sim";
+  start cmd "1.plan-runner";
+  start cmd "2.state-translator";
+  start cmd "4.director";
+
+  start group "imitation";
+
+}
diff --git a/data/rosbag/iiwa/sr300_box.bag b/data/rosbag/iiwa/sr300_box.bag
diff --git a/doc/coordinate_conventions.md b/doc/coordinate_conventions.md
@@ -0,0 +1,40 @@
+# Coordinate Conventions in pytorch-dense-correspondence
+
+Keeping coordinate systems consistent is not a spontaneous event.
+
+There are a few reasons this is not always straightforward:
+
+- Typical computer vision "x,y,z" convention in 3D space is "right-down-forward"
+- Associated with this right-down-forward convention, it's easiest to think about pixel positions 
+as being associated with this right, down convention.  This gives us the "(u, v)" convention for defining
+pixel coordinates.
+- On the other hand, images that are arrays/tensors in numpy or torch are accessed as row, column, in that order.
+That isn't consisent with the (u, v) convention
+- Also on the other hand, typical robot convention is to think of "x" as being forward, away from the robot, and "y" to the left.
+This gives "forward-left-up" for "x,y,z"
+
+## Conventions
+
+In this repo we would like to follow this simple convention for cartesian 3D frames:
+
+- <b>All cartesian frames are specified in right-down-forward coordinates</b>.
+
+But we currently have a more hybrid / mixed approach with accessing pixel data:
+
+- <b>When doing 3D vision, we use the OpenCV "(u,v)" convention for pixel coordinates.  This aligns with our
+cartesian coordinate system as follows in this diagram:
+
+<p align="center">
+  <img src="./OpenCVcoordinates.png" width="450"/>
+</p>
+
+- <b>All data stored as numpy arrays or torch tensors should be stored in row, column format</b>.  This means
+that in order to access the correct pixel in this data, if using (u,v) coordinates, you should access the data
+as:
+
+    ```python
+    pixel_at_u_v = data_tensor[v,u]
+    ```
+- To convert between (u,v) and single index the formulas are n = u + image_width * v. Similarly (u,v) = (n % image_width, n / image_width).
+
+
diff --git a/doc/gripper_and_palm_frame.png b/doc/gripper_and_palm_frame.png
diff --git a/doc/images/OpenCVcoordinates.png b/doc/images/OpenCVcoordinates.png
diff --git a/doc/vive_guide.md b/doc/vive_guide.md
@@ -1,12 +1,15 @@
 # Vive Teleop
 This is a guide to using the HTC vive headset and controllers to teleop the robot in simulation or using hardware.
 
-# Prerequisites
+# Prerequisites for VR computer
 - Follow the [HTC Vive Installation Guide](https://support.steampowered.com/kb_article.php?ref=2001-UXCM-4439)
 - Install [Steam](https://store.steampowered.com/about/) and SteamVR
-- Install Spartan using docker and checkout branch vive-teleop
+- Clone spartan
 
-# Install required packages
+# Prerequisites for robot computer (run on same computer if using simulation)
+- Install spartan using docker
+
+# Install required packages (VR computer)
 Run the following commands to install required packages
 ```
 sudo apt-get install libudev-dev libvulkan-dev libsdl2-dev libglfw3-dev libssl-dev zlib1g-dev python-pip ros-kinetic-tf ros-kinetic-tf2*
@@ -16,7 +19,7 @@ pip install openvr
 
 Additionally, clone the [OpenVR repository](https://github.com/ValveSoftware/openvr) into a known location
 
-# Install ROS Plugins using catkin
+# Install ROS Plugins using catkin (VR computer)
 The Plugin is build using catkin. You have to create a catkin workspace first. Open up a terminal and:
 ```
 mkdir -p catkin_ws/src
@@ -34,12 +37,42 @@ cd ..
 catkin_make
 source devel/setup.bash
 ```
-# Run Vive Telop
+# Run Vive Teleop Simulation
 1. Start SteamVR and ensure that headset and controllers are tracking
-2. Start spartan and run simulation or robot setup through procman
-3. Source your catkin workspace and run `roslaunch htc_vive_teleop_stuff htc_vive_tf_and_joy.launch`
-4. Close the instance of rviz opened by procman and start it outside the docker container
-  - Source your catkin workspace
+2. In a new terminal, navigate to your spartan directory
+  - `./setup/docker/docker_run.py -nethost`
+  - In docker, start procman and run start simulation
+  - Through procman start the process *vive-teleop*
+3. In a new terminal, navigate to your catkin workspace
+  - `source devel/setup.bash`
+  - `roslaunch htc_vive_teleop_stuff htc_vive_tf_and_joy.launch`
+4. Close the instance of rviz opened by procman
+5. In a new terminal, navigate to your catkin workspace
+  - `source devel/setup.bash`
   - cd to your spartan folder and run `source build/setup_environment.sh`
-  - Run `rosrun rviz rviz -d $SPARTAN_SOURCE_DIR/src/catkin_projects/station_config/RLG_iiwa_1/rviz_vive.rviz";`
-5. Through procman start the process *vive-teleop*
+  - `rosrun rviz rviz -d $SPARTAN_SOURCE_DIR/src/catkin_projects/station_config/RLG_iiwa_1/rviz_vive.rviz`
+6. Put on the vive headset and look around. Press and hold the trackpad to move the arm and hold the trigger to close the gripper. Press the menu button (above trackpad) to play / pause point clouds.
+
+# Run Vive Teleop on Robot
+On robot computer:
+1. Run `hostname -I` and look at the first entry to determine your ip address (should be 128.30.27.155 for kuka2). Write this number anywhere you see ROBOT_IP_HERE.
+1. Navigate to your spartan directory and run `./setup/docker/docker_run.py -nethost`
+2. Run `export ROS_IP=ROBOT_IP_HERE`
+3. Start procman and set up robot normally
+4. Through procman start the process *vive-teleop*
+
+On VR computer:
+1. Run `hostname -I` and look at the first entry to determine your ip address (should be 128.30.xx.xxx). Write this number anywhere you see VR_IP_HERE.
+2. Start SteamVR and ensure that headset and controllers are tracking
+3. In a new terminal, navigate to your catkin workspace
+  1. `export ROS_MASTER_URI=http://ROBOT_IP_HERE:11311`
+  2. `export ROS_IP=VR_IP_HERE`
+  3. `source devel/setup.bash`
+  4. `roslaunch htc_vive_teleop_stuff htc_vive_tf_and_joy.launch`
+4. In a new terminal, navigate to your catkin workspace
+  1. `export ROS_MASTER_URI=http://ROBOT_IP_HERE:11311`
+  2. `export ROS_IP=VR_IP_HERE`
+  3. `source devel/setup.bash`
+  4. cd to your spartan folder and run `source build/setup_environment.sh`
+  5. `rosrun rviz rviz -d $SPARTAN_SOURCE_DIR/src/catkin_projects/station_config/RLG_iiwa_1/rviz_vive.rviz`
+5. Put on the vive headset and look around. Press and hold the trackpad to move the arm and hold the trigger to close the gripper. Press the menu button (above trackpad) to play / pause point clouds.