Update README and requirements.

README.md

@@ -27,6 +27,7 @@ and avoiding dynamic obstacles like thrown balls:
 * `upright_ros_interface`: Tools for ROS communication. These can be useful in
   simulation for multi-processing, or to support real hardware.
 * `upright_sim`: (PyBullet) simulation environments for balancing objects.
+* `upright_robust`: Robust planning for balancing objects with uncertain inertial parameters. See [here](upright_robust/README.md) for more details.
 
 ## Setup and Installation
 
@@ -149,10 +150,12 @@ You may wish to record the results in a bag file using the
 Some packages contain tests. Python tests use [pytest](https://pytest.org/).
 Run `pytest .` inside a package's `tests` directory to run the Python tests.
 
-## Citation
+## Citations
 
-If you find this work useful, feel free to cite the accompanying
-[paper](https://doi.org/10.1109/LRA.2023.3324520):
+If you find this work useful, feel free to cite (one of) the accompanying
+papers.
+
+The [original paper](https://doi.org/10.1109/LRA.2023.3324520) is:
 ```
 @article{heins2023upright,
   title={Keep It Upright: Model Predictive Control for Nonprehensile Object Transportation With Obstacle Avoidance on a Mobile Manipulator}, 
@@ -166,6 +169,9 @@ If you find this work useful, feel free to cite the accompanying
 }
 ```
 
+We have also recently developed a follow-up work on robust planning under
+inertial parameter uncertainty (link coming soon).
+
 ## License
 
 MIT (see the LICENSE file).

requirements.txt

@@ -1,6 +1,7 @@
 pybullet >= 3.2.0
-git+https://github.com/adamheins/pyb_utils@main#egg=pyb_utils
 numpy >= 1.21.5
 scipy >= 1.10.0
 spatialmath-python >= 1.0.0
 xacrodoc >= 0.4.0
+pyb_utils >= 2.2.0
+git+https://github.com/utiasDSL/rigeo@main#egg=rigeo

upright_robust/README.md

@@ -1,62 +1,23 @@
 # Robust Nonprehensile Object Transportation
 
 Nonprehensile object transportation that is robust to uncertainty in the
-inertial parameters (mass, center of mass, inertia matrix).
-
-## Configuration
-
-Tilting type:
-
-* **tray**: rotate the tray so its normal vector is aligned with total
-  acceleration, neglecting the object CoMs, but constraints are still enforced
-  to avoid dropping them
-* **tray_only**: same as tray but without any balancing constraints
-* **full**: take all objects into accounting when tilting/rotating
-* **flat**: keep the tray flat
-
-Constraint type:
-
-* **nominal**: nominal balancing constraints based on some guess of the
-  inertial parameters, does not account for uncertainty
-* **robust**: balancing constraints robust to uncertainty in the inertial
-  parameters
-* **approx_robust**: instead of solving the full robust QP, solve the nominal
-  one and then just scale the resulting acceleration to satisfy the robust
-  balancing constraints. This is useful when there are many (3+) objects and
-  the QP becomes too slow to solve at real-time rates.
-
-If `reactive.face_form=true`, the face form of the robust constraints is used
-(rather than the original span form).
-
-To remove all balancing constraints, set `balancing.enabled=false`.
-
-## SDP relaxation
-
-The work on SDP relaxations can be found in `scripts/relaxation`. The
-relaxations integrated with the config files can be found in
-`sdp_relaxation.py`. Use `--verify` to verify that a single approximate inertia
-value with always hold, or use `--elimination` to detect redundant constraints
-that can be eliminated.
-
-## Simulation
-
-To run the simulation normally use `scripts/simulation.py`.
-
-To run the simulation using ROS to talk to the controller, ensure ROS master is
-running. Then run the simulation using `scripts/ros_simulation.py` followed by
-the controller using `scripts/ros_controller.py`.
-
-## Hardware
-
-When first starting out, one needs to build up task complexity gradually to
-ensure things work as expected:
-1. Use `nominal_flat/short.yaml` after having modified the goal waypoint to
-   zero, in order to remain stationary.
-2. Use `nominal_full/short.yaml`, again with stationary trajectory, to ensure
-   balancing constraints are okay with rotation capabilities.
-3. Repeat 1 and 2, slowly increasing the waypoint distance to 2 meters.
-
-Other notes:
-* The KF may need tuning.
-* Using an actual pre-planned trajectory does not make too much sense, since we
-  do not know how fast the object can actually travel.
+inertial parameters (mass, center of mass, inertia matrix). We use the same
+optimal control problem formulation as the original work in this repository,
+but instead solve the problem once offline with a long time horizon. The plan
+is then tracked online.
+
+## Simulations
+
+* Run the simulation experiments with `scripts/planning_sim_loop.py`.
+* Process the results (i.e., verify robustness) using
+  `scripts/process_sim_runs.py`.
+
+## Experiments
+
+The node for running the planner online with the real robot lives in the
+`upright_ros_interface` package, to keep all of the ROS infrastructure
+contained in one place. To run it, do
+```
+roslaunch upright_ros_interface track_plan.launch config:=<path to yaml file>
+```
+The plan is generated and then tracked online the robot.