improve documentations for real robot experiment

docs/real_franka.md

@@ -6,12 +6,7 @@ When running with a real robot, a separate gym env is needed. For our examples,
 
 ![](./images/robot_infra_interfaces.png)
 
-This requires the installation of the following packages:
-
-- [serl_franka_controller](https://github.com/rail-berkeley/serl_franka_controller)
-- `serl_robot_infra`: [readme](serl_robot_infra/README.md)
-
-Follow the README in `serl_robot_infra` for basic robot operation instructions.
+Follow the [README](serl_robot_infra/README.md) in `serl_robot_infra` for installation and basic robot operation instructions.
 
 
 *NOTE: The following code will not run as it is, since it will require custom data, checkpoints, and robot env. We provide the code as a reference for how to use SERL with real robots. Learn this section in incremental order, starting from the first task (peg insertion) to the last task (bin relocation). Modify the code according to your needs. *
@@ -31,20 +26,32 @@ The peg insertion task is best for getting started with running SERL on a real r
 ### Procedure
 1. 3D-print (1) **Assembly Object** of choice and (1) corresponding **Assembly Board** from the **Single-Object Manipulation Objects** section of [FMB](https://functional-manipulation-benchmark.github.io/files/index.html). Fix the board to the workspace and grasp the peg with the gripper.
 2. 3D-print (2) wrist camera mounts for the RealSense D405 and install onto the threads on the Robotiq Gripper. Create your own config from [peg_env/config.py](../serl_robot_infra/franka_env/envs/peg_env/config.py), and update the camera serial numbers in `REALSENSE_CAMERAS`.
-3. The reward is given by checking the end-effector pose matches a fixed target pose. Manually move the arm into a pose where the peg is inserted into the board and update the `TARGET_POSE` in [peg_env/config.py](../serl_robot_infra/franka_env/envs/peg_env/config.py) with the measured end-effector pose.
-4. Set `RANDOM_RESET` to `False` inside the config file to speedup training. Note the policy would only generalize to any board pose when this is set to `True`, but only try this after the basic task works.
-5. Record 20 demo trajectories with the spacemouse.
+3. Adjust for the weight of the wrist camera by editing `Desk > Settings > End-effector > Mechnical Data > Mass`.
+4. Unlock the robot and activate FCI in Desk. Then, start the franka_server by running:
+    ```bash
+    python serl_robo_infra/robot_servers/franka_server.py --gripper_type=<Robotiq|Franka|None> --robot_ip=<robot_IP> --gripper_ip=<[Optional] Robotiq_gripper_IP>
+    ```
+    This should start the impedance controller and a Flask server ready to recieve requests.
+5. The reward in this task is given by checking whether the end-effector pose matches a fixed target pose. Grasp the desired peg with  `curl -X POST http://127.0.0.1:5000/close_gripper` and manually move the arm into a pose where the peg is inserted into the board. Print the current pose with `curl -X POST http://127.0.0.1:5000/getpos` and update the `TARGET_POSE` in [peg_env/config.py](../serl_robot_infra/franka_env/envs/peg_env/config.py) with the measured end-effector pose.
+
+    **Note: the `getpos` command prints the pose in xyz+quaternion format, but `TARGET_POSE` expects xyz+euler(rpy) format. Please use your favourite quat to euler calculator to do the conversion.
+
+    **Note: make sure the wrist joint is centered (away from joint limits) and z-axis euler angle is positive at the target pose to avoid discontinuities.
+
+6. Set `RANDOM_RESET` to `False` inside the config file to speedup training. Note the policy would only generalize to any board pose when this is set to `True`, but only try this after the basic task works.
+7. Record 20 demo trajectories with the spacemouse.
     ```bash
+    cd examples/async_peg_insert_drq
     python record_demo.py
     ```
     The trajectories are saved in `examples/async_peg_insert_drq/peg_insertion_20_trajs_{UUID}.pkl`.
-6. Train the RL agent with the collected demos by running both learner and actor nodes.
+8. Edit `demo_path` and `checkpoint_path` in `run_learner.sh` and `run_actor.sh`. Train the RL agent with the collected demos by running both learner and actor nodes.
     ```bash
     bash run_learner.sh
     bash run_actor.sh
     ```
-7. If nothing went wrong, the policy should converge with 100% success rate within 30 minutes without `RANDOM_RESET` and 60 minutes with `RANDOM_RESET`.
-8. The checkpoints are automatically saved and can be evaluated with:
+9. If nothing went wrong, the policy should converge with 100% success rate within 30 minutes without `RANDOM_RESET` and 60 minutes with `RANDOM_RESET`.
+10. The checkpoints are automatically saved and can be evaluated by setting the `--eval_checkpoint_step=CHECKPOINT_NUMBER_TO_EVAL` and `--eval_n_trajs=N_TIMES_TO_EVAL` flags in `run_actor.sh`. Then run:
     ```bash
     bash run_actor.sh
     ```

serl_robot_infra/README.md

@@ -1,24 +1,24 @@
 # SERL Robot Infra
+![](../docs/images/robot_infra_interfaces.png)
 
 All robot code is structured as follows:
 There is a Flask server which sends commands to the robot via ROS. There is a gym env for the robot which communicates with the Flask server via post requests.
 
 - `robot_server`: hosts a Flask server which sends commands to the robot via ROS
 - `franka_env`: gym env for the robot which communicates with the Flask server via post requests
 
-```mermaid
-graph LR
-A[Robot] <-- ROS --> B[Robot Server]
-B <-- HTTP --> C[Gym Env]
-C <-- Lib --> D[RL Policy]
-```
 
 ### Installation
 
-First, make sure the NUC meets the specifications [here](https://frankaemika.github.io/docs/requirements.html).
+1. Install `libfranka` and `franka_ros` with instructions [here](https://frankaemika.github.io/docs/requirements.html).
 
-Then install the `serl_franka_controllers` from: https://github.com/rail-berkeley/serl_franka_controllers
+2. Then install the `serl_franka_controllers` from https://github.com/rail-berkeley/serl_franka_controllers
 
+3. Then, install this package and it's dependencies.
+    ```bash
+    conda activate serl
+    pip install -e .
+    ```
 ### Usage
 
 **Robot Server**
@@ -27,12 +27,49 @@ To start using the robot, first power on the robot (small switch on the back of
 
 From there you should be able to navigate to `serl_robot_infra` and then simply run the franka server. This requires to be in a ROS environment.
 
-```py
-python serl_robot_infra/robot_servers/franka_server.py
+```bash
+conda activate serl
+python serl_robo_infra/robot_servers/franka_server.py --gripper_type=<Robotiq|Franka|None> --robot_ip=<robot_IP> --gripper_ip=<[Optional] Robotiq_gripper_IP>
 ```
 
 This should start ROS node impedence controller and the HTTP server. You can test that things are running by trying to move the end effector around, if the impedence controller is running it should be compliant.
 
+The HTTP server is used to communicate between the ROS controller and gym environments. Possible HTTP requests include:
+
+| Request | Description |
+| --- | --- |
+| startimp | Stop the impedance controller |
+| stopimp | Start the impedance controller |
+| pose | Command robot to go to desired end-effector pose given in base frame (xyz+quaternion) |
+| getpos | Return current end-effector pose in robot base frame (xyz+rpy)|
+| getvel | Return current end-effector velocity in robot base frame |
+| getforce | Return estimated force on end-effector in stiffness frame |
+| gettorque | Return estimated torque on end-effector in stiffness frame |
+| getq | Return current joint position |
+| getdq | Return current joint velocity |
+| getjacobian | Return current zero-jacobian |
+| getstate | Return all robot states |
+| jointreset | Perform joint reset |
+| activate_gripper | Activate the gripper (Robotiq only) |
+| reset_gripper | Reset the gripper (Robotiq only) |
+| get_gripper | Return current gripper position |
+| close_gripper | Close the gripper completely |
+| open_gripper | Open the gripper completely |
+| move_gripper | Move the gripper to a given position (Robotiq only) |
+| clearerr | Clear errors |
+| update_param | Update the impedance controller parameters |
+
+These commands can also be called in terminal. Useful ones include:
+```bash
+curl -X POST http://127.0.0.1:5000/activate_gripper # Activate gripper
+curl -X POST http://127.0.0.1:5000/close_gripper # Close gripper
+curl -X POST http://127.0.0.1:5000/open_gripper # Open gripper
+curl -X POST http://127.0.0.1:5000/getpos # Print current end-effector pose
+curl -X POST http://127.0.0.1:5000/jointreset # Perform joint reset
+curl -X POST http://127.0.0.1:5000/stopimp # Stop the impedance controller
+curl -X POST http://127.0.0.1:5000/startimp # Start the impedance controller (**Only run this after stopimp**)
+```
+
 **Robot Env (Client)**
 
 Lastly, we use a gym env interface to interact with the robot server, defined in this repo under `franka_env`. Simply run `pip install -e .` in the `robot_infra` directory, and in your code simply initialize the env via `gym.make("Franka-{ENVIRONMENT NAME}-v0)`.

serl_robot_infra/robot_servers/franka_server.py

@@ -18,7 +18,9 @@
 flags.DEFINE_string(
     "robot_ip", "172.16.0.2", "IP address of the franka robot's controller box"
 )
-flags.DEFINE_string("gripper_ip", "192.168.1.114", "IP address of the robotiq gripper")
+flags.DEFINE_string(
+    "gripper_ip", "192.168.1.114", "IP address of the robotiq gripper if being used"
+)
 flags.DEFINE_string(
     "gripper_type", "Robotiq", "Type of gripper to use: Robotiq, Franka, or None"
 )
@@ -326,7 +328,7 @@ def get_state():
             }
         )
 
-    # precision mode for reset
+    # Route for updating compliance parameters
     @webapp.route("/update_param", methods=["POST"])
     def update_param():
         reconf_client.update_configuration(request.json)