Add manipulation code and test base motion + grasping via ROS (#6)

* iadd motion files, interpolation, rrt placeholder, and ROS launch files * add ROS tools, mask rccnn wrapper, and utils * update configs for planning and executing * update configuration files# Please enter the commit message for your changes. Lines starting * add the pick test * add ros services and fix * version consistency so we can use it as a ROS package * update readme * add hab stretch - contains URDF for planning * update * add new readm * update third party config and add repos * require detectron for the full perception install * reformat with black * Formatting * Run pytest in tests folder only * rename the file * Formatting * add a new file containing point cloud tools * update pt cloud tools - make sure its ok * Fix imports * Add modifications required for demo to run * Restructure files, update README * Formatting * Update instructions based on Ben's feedback * README formatting * Make mamba optional * Clean up troubleshooting * Debug init node issue * Add comments to script Co-authored-by: ExhAustin <[email protected]>
facebookresearch · Nov 22, 2022 · 4fcfcd7 · 4fcfcd7
1 parent caccb0b
commit 4fcfcd7
Show file tree

Hide file tree

Showing 72 changed files with 8,890 additions and 66 deletions.
diff --git a/.circleci/config.yml b/.circleci/config.yml
@@ -41,10 +41,10 @@ jobs:
             source ~/.bashrc
             conda activate ~/testenv
             pip install -e .
-            pytest
+            pytest tests/
 
 workflows:
   main:
     jobs:
       - formatting
-      - integration-sim
+      - integration-sim
diff --git a/.gitmodules b/.gitmodules
@@ -0,0 +1,6 @@
+[submodule "hab_stretch"]
+	path = assets/hab_stretch
+	url = [email protected]:cpaxton/hab_stretch.git
+[submodule "third_party/contact_graspnet"]
+	path = third_party/contact_graspnet
+	url = [email protected]:cpaxton/contact_graspnet.git
diff --git a/README.md b/README.md
@@ -7,42 +7,69 @@ Mostly Hello Stretch infrastructure
 
 ## Installation
 
-1. Prepare a conda environment (ex: `conda create -n home_robot python=3.8`)
-1. Install Mamba (needed for MRP): `conda install -c conda-forge mamba`
+1. Prepare a conda environment (ex: `conda create -n home_robot python=3.8 && conda activate home_robot`)
+1. Install Mamba (optional but highly recommended): `conda install -c conda-forge mamba`
 1. Install repo `pip install -e .`
 
 ### Additional instructions for setting up on hardware
 
-1. Setup the Stretch robot following official instructions [here](https://github.com/hello-robot/stretch_install)
-1. Install stretch_ros following official instructions [here](https://github.com/hello-robot/stretch_ros/blob/dev/noetic/install_noetic.md)
-1. Install Hector SLAM: `sudo apt install ros-noetic-hector-*`
+1. Install firmware from Hello Robot
+    ```sh
+    # Copy robot factory data into your user workspace
+    cp -r /etc/hello-robot/stretch-re* ~
 
-#### Conflicting Processes Already Running
+    # Clone the official setup scripts
+    cd ~
+    git clone https://github.com/hello-robot/stretch_install
+    cd stretch_install
 
-- Restart stretch
-- See running processes using:
+    # Run setup script (DO NOT RUN BOTH)
+    ./stretch_new_robot_installation.sh  # if installing into a new robot
+    ./stretch_new_user_installation.sh  # if installing into a new user account on a already-set-up robot
+    ```
+1. Open `~/.bashrc`. You will see a block of commands that initializes Stretch, and another block that initializes Conda. If needed, move the stretch setup block BEFORE the conda initialization.
+1. Launch a new bash shell. Activate an conda env with Python 3.8 installed.
+1. Link `home_robot` and install ROS stack
+    ```sh
+    # Create symlink in catkin workspace
+    ln -s /abs/path/to/home-robot/rospkg $HOME/catkin_ws/src/home_robot
+
+    # Install dependencies for catkin
+    pip install empy catkin_pkg rospkg
+
+    # Build catkin workspace
+    cd ~/catkin_ws
+    catkin_make
+
+    # Add newly built setup.bash to .bashrc
+    echo "source ~/catkin_ws/devel/setup.bash" > ~/.bashrc
+    ```
+1. Calibrate robot following instructions [here](https://github.com/hello-robot/stretch_ros/tree/master/stretch_calibration#checking-the-current-calibration-with-new-observations).
+1. Generate URDF from calibration data: `rosrun stretch_calibration update_urdf_after_xacro_change.sh`.
+1. Run `stretch_robot_system_check.py` to make sure that things are normal.
+
+#### Additional hardware stack dependencies
+1. Hector SLAM: `sudo apt install ros-noetic-hector-*`
+1. (For grasping only) Detectron 2: `python -m pip install 'git+https://github.com/facebookresearch/detectron2.git'`
+
+You also need to install a supported grasp prediction library. (TODO: clarify?)
 
-```sh
-mrp info
-```
 
 ## Usage
 
 ### Launching the hardware stack:
 ```sh
-cd src/home_robot
-mrp up hw_stack
-```
+# Launch core components
+roslaunch home_robot startup_stretch_hector_slam.launch
 
-This launches:
-- Stretch ROS driver
-- Hector SLAM
-- State estimation node
-- Continuous controller node
+# Launch state estimator & goto controller
+cd /path/to/home-robot/src/home_robot
+mrp up agent_procs
+```
 
 ### Launching a minimal kinematic simulation (no camera yet)
 ```sh
-cd src/home_robot
+cd /path/to/home-robot/src/home_robot
 mrp up sim_stack
 ```
 
@@ -51,6 +78,11 @@ This launches:
 - State estimation node
 - Continuous controller node
 
+### Stopping all processes
+```sh
+mrp down
+```
+
 ### Launching a simple local command line interface (CLI) on the robot:
 
 The CLI currently exposes a simple base control interface via the terminal.
@@ -79,9 +111,39 @@ robot.set_goal(]1.0, 0.0, 0.0])  # Sets XYZ target
 robot.get_base_state()  # Shows the robot's XYZ coordinates
 ```
 
-### Stopping all processes
-```sh
-mrp down
+### Launching ROS Demo
+
+You need to create a catkin workspace on your server in order to run this demo, as this is where we will run [Contact Graspnet](https://github.com/cpaxton/contact_graspnet/tree/cpaxton/devel).
+
+Contact graspnet is downloaded under `third_party/`, but there is a `CATKIN_IGNORE` file in this directory. You want to symlink this file out into your workspace:
+```
+ROSWS=/path/to/ros_ws
+ln -s `rospack find home_robot`/third_party/contact_graspnet $ROSWS/src/contact_graspnet
+```
+... but it actually shouldn't be necessary. What is necessary is to build the grasp service defined in `home_robot` by placing it into `$ROSWS`.
+
+
+Put the robot in its initial position, e.g. so the arm is facing cups you can pick up. On the robot side:
+```
+roslaunch home_robot startup_stretch_hector_slam.launch
+```
+
+### Troubleshooting 
+
+- `ImportError: cannot import name 'gcd' from 'fractions'`: Launch ros nodes from an env with Python 3.8 instead of 3.9
+- Conflicting Processes Already Running: `mrp down`, restart robot if that doesn't work.
+
+
+## Third Party Code
+
+#### Contact GraspNet
+
+Contact graspnet is supported as a way of generating candidate grasps for the Stretch to use on various objects. We have our own fork of [Contact Graspnet](https://github.com/cpaxton/contact_graspnet/tree/cpaxton/devel) which has been modified with a ROS interface.
+
+Follow the installation instructions as normal and start it with:
+```
+conda activate contact_graspnet_env
+~/src/contact_graspnet$ python contact_graspnet/graspnet_ros_server.py  --local_regions --filter_grasps
 ```
 
 ## Code Contribution

diff --git a/assets/blue_block.urdf b/assets/blue_block.urdf
@@ -0,0 +1,21 @@
+<?xml version="1.0"?>
+<robot name="simple_table">
+
+	<material name="color">
+		<color rgba=".01 .26 .87 1"/>
+	</material>
+
+	<link name="base_link">
+		<visual>
+			<geometry>
+				<box size="0.05 0.05 0.05"/>
+			</geometry>
+			<material name="color"/>
+		</visual>
+		<collision>
+			<geometry>
+				<box size="0.05 0.05 0.05"/>
+			</geometry>
+		</collision>
+	</link>
+</robot>
diff --git a/assets/green_block.urdf b/assets/green_block.urdf
@@ -0,0 +1,21 @@
+<?xml version="1.0"?>
+<robot name="simple_table">
+
+	<material name="color">
+		<color rgba="0.082 .69 .10 1"/>
+	</material>
+
+	<link name="base_link">
+		<visual>
+			<geometry>
+				<box size="0.05 0.05 0.05"/>
+			</geometry>
+			<material name="color"/>
+		</visual>
+		<collision>
+			<geometry>
+				<box size="0.05 0.05 0.05"/>
+			</geometry>
+		</collision>
+	</link>
+</robot>
diff --git a/assets/hab_stretch b/assets/hab_stretch
diff --git a/assets/large_table.urdf b/assets/large_table.urdf
@@ -0,0 +1,21 @@
+<?xml version="1.0"?>
+<robot name="simple_table">
+
+	<material name="dark">
+		<color rgba="0.4 0.4 0.4 1"/>
+	</material>
+
+	<link name="base_link">
+		<visual>
+			<geometry>
+				<box size="1.4 1.4 0.8"/>
+			</geometry>
+			<material name="dark"/>
+		</visual>
+		<collision>
+			<geometry>
+				<box size="1.4 1.4 0.8"/>
+			</geometry>
+		</collision>
+	</link>
+</robot>