Merge pull request #42 from tud-airlab/ft-add-pushing

Ft add pushing

README.md

@@ -1,39 +1,62 @@
+# Overview
+This is an implementation of a Model Predictive Path Integral (MPPI) controller which uses IsaacGym as dynamical model for the rollouts. We provide several examples of what tasks you can solve with it: 
+
+<p align="center">
+<img src="docs/source/overview_gif.gif"/>
+</p>
+
+You can extend the repository with your robots and tasks since it is designed to be modular and re-usable. 
+
 # Installation
 
-This project requires the sourcec code of isaacgym inside the folder
-thirdparties. Download it from https://developer.nvidia.com/isaac-gym and place
-it there. Then you can proceed with the installation descibed below.
+*NOTE: To use the GPU pipeline (default) you need an NVIDIA graphics card. If you do not have one, you can use the CPU pipeline, with less samples.*
 
-## Developers
+This project requires the source code of IsaacGym inside the folder
+`thirdparties`. Download it from https://developer.nvidia.com/isaac-gym, extract it, and place
+it in `mppi-isaac/thirdparty`. Then you can proceed with the installation descibed below. 
 
-For the time being, you can install useful developer software using in a poetry virtual environment:
+## Virtual environment (advised)
+You can install the necesary dependancies using [poetry](https://python-poetry.org/docs/) virtual environment. After installing poetry, move in `mppi-isaac` and run
 ```bash
 poetry install --with dev
 ```
 Bare in mind that the installation might take several minutes the first time. But it's worth it.
 
-Later on, you should also be able to just use [poetry](https://python-poetry.org/docs/) to install the package with:
-```bash
-poetry install
-```
-
 Access the virtual environment using
 ```bash
 poetry shell
 ```
 
-Alternatively, you can also install at the system level using pip:
+### **Test the installation**
+Test that everything is properly set-up, use pytest
+```bash
+cd examples
+poetry run pytest
+```
+## System-level installation
+Alternatively, you can also install at the system level using pip, eben though we advise using the virtual environment:
 ```bash
 pip install .
 ```
 
-## Testing
+## Troubleshooting
+If you have an Nvidia card and after running the simulation you get a black screen, you might need to force the use of the GPU card through ``export VK_ICD_FILENAMES=/usr/share/vulkan/icd.d/nvidia_icd.json``. Run this command from the same folder as the script to be launched
 
-Test changes using pytest
-```bash
-cd examples
-poetry run pytest
+# Running the examples
+Access the virtual environment if installed with poetry (with `poetry shell`). You can run two types of examples, either the ones using IsaacGym or the ones using pybullet. In the `examples` folder you find all the scripts. The ones realed to IsaacGym contains either "server" or "client" in the name. 
+
+## IsaacGym examples
+To run the examples with IsaacGym (for example for non-prehensile pushing and whole body control), you need two terminals because it is required to run a "server" and a "client" script. In the first one run the server:
+```bash 
+python3 heijn_push_server.py
+```
+In the second one the client:
+```bash 
+python3 heijn_push_client.py
 ```
 
-## Troubleshooting
-If you have an Nvidia card and after running the simulation you get a black screen, you might need to force the use of the GPU card through ``export VK_ICD_FILENAMES=/usr/share/vulkan/icd.d/nvidia_icd.json``. Run this command from the same folder as the script to be launched
+## Pybullet examples
+Simply run one of the example scripts which use pybullet, for instance for the panda robot:
+```bash
+python3 panda_robot_with_obstacles.py
+```

assets/urdf/boxer/boxer.urdf

@@ -204,7 +204,7 @@ to be that way. -->
     </visual>
   </link>
   <joint name="ee_joint" type="fixed">
-    <origin rpy="0 0 0" xyz="0 -0.4 0"/>
+    <origin rpy="0 0 0" xyz="0 -0.3 0.1"/>
     <parent link="base_link"/>
     <child link="ee_link"/>
   </joint>

assets/urdf/heijn/heijn.urdf

@@ -23,21 +23,21 @@
   <link name="world"/>
   <link name="base_link_x">
     <inertial>
-      <mass value="1.0"/>
+      <mass value="2.0"/>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
     </inertial>
   </link>
   <link name="base_link_y">
     <inertial>
-      <mass value="1.0"/>
+      <mass value="2.0"/>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
     </inertial>
   </link>
   <link name="base_link">
     <inertial>
-      <mass value="40.0"/>
+      <mass value="2.0"/>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
     </inertial>
@@ -57,7 +57,7 @@
   </link>
   <link name="pole_link">
     <inertial>
-      <mass value="1.0"/>
+      <mass value="0.3"/>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
     </inertial>
@@ -77,7 +77,7 @@
   </link>
   <link name="platform_link">
     <inertial>
-      <mass value="1.0"/>
+      <mass value="2.0"/>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
     </inertial>

conf/actors/boxer.yaml

@@ -2,7 +2,6 @@ type: robot
 name: boxer
 urdf_file: "boxer/boxer.urdf"
 gravity: true
-mass: 100.00
 fixed: false
 flip_visual: false
 differential_drive: true

conf/actors/omnipanda.yaml

@@ -0,0 +1,7 @@
+type: robot
+name: omnipanda
+urdf_file: "omni_panda/omniPandaWithGripper.urdf"
+ee_link: "panda_ee_tip"
+fixed: True
+flip_visual: true
+gravity: false

conf/config_boxer_push.yaml

@@ -0,0 +1,11 @@
+defaults:
+  - mppi: boxer_push
+  - isaacgym: push
+
+goal: [2.0, 2.0]
+render: true
+n_steps: 1
+nx: 4
+
+actors: ['boxer']
+initial_actor_positions: [[0.0, 2.0, 0.0]]

conf/config_heijn_push.yaml

@@ -0,0 +1,11 @@
+defaults:
+  - mppi: heijn_push
+  - isaacgym: push
+
+goal: [2.0, 2.0]
+render: true
+n_steps: 5
+nx: 6
+
+actors: ['heijn']
+initial_actor_positions: [[0.0, 0.0, 0.05]]

conf/config_omnipanda.yaml

@@ -0,0 +1,11 @@
+defaults:
+  - mppi: omnipanda
+  - isaacgym: push
+
+goal: [0.5, 0.4, 0.7]
+render: true
+n_steps: 5
+nx: 24
+
+actors: ['omnipanda']
+initial_actor_positions: [[0.0, 0.0, 0.0]]

conf/isaacgym/push.yaml

@@ -2,3 +2,4 @@ defaults:
   - base_isaacgym
 
 dt: 0.04
+substeps: 1

conf/isaacgym/slow.yaml

@@ -1,5 +1,5 @@
 defaults:
   - base_isaacgym
 
-dt: 0.01
+dt: 0.02
 substeps: 1

conf/mppi/boxer_push.yaml

@@ -0,0 +1,17 @@
+defaults:
+  - base_mppi
+
+mppi_mode: "halton-spline"  # halton-spline, simple
+sampling_method: "halton"   # halton, random
+num_samples: 400
+horizon: 12               # At least 12 for Halton Sampling
+device: "cuda:0"
+lambda_: 0.01
+u_min: [-1.2, -3.5]
+u_max: [1.2, 3.5]
+noise_sigma: [[2., 0.], [0., 8.]]
+update_cov: False
+rollout_var_discount: 0.95
+sample_null_action: True
+noise_abs_cost: False
+filter_u: True

conf/mppi/heijn_push.yaml

@@ -0,0 +1,18 @@
+defaults:
+  - base_mppi
+
+mppi_mode: "halton-spline"  # halton-spline, simple
+sampling_method: "halton"   # halton, random
+num_samples: 300
+horizon: 8                  
+device: "cuda:0"
+lambda_: 0.05
+u_min: [-0.8, -0.8, -1.0]
+u_max: [0.8, 0.8, 1.0]
+noise_sigma: [[0.5, 0., 0.], [0., 0.5, 0.], [0., 0., 1.8]]
+update_cov: False
+rollout_var_discount: 0.95
+sample_null_action: True
+noise_abs_cost: False
+filter_u: False
+use_priors: False

conf/mppi/omnipanda.yaml

@@ -0,0 +1,28 @@
+defaults:
+  - base_mppi
+
+mppi_mode: "halton-spline"  # halton-spline, simple
+sampling_method: "halton"   # halton, random
+num_samples: 500
+horizon: 6                # At least 12 for Halton Sampling
+device: "cuda:0"
+u_min: [-0.4, -0.4, -0.4, -0.5, -0.5, -0.5, -0.5, -0.5, -0.5, -0.5, -0.1, -0.1]
+u_max: [0.4, 0.4, 0.4, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.1, 0.1]
+lambda_: 0.1
+noise_sigma: [[0.5, 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.],
+              [0., 0.5, 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.],
+              [0., 0., 0.5, 0., 0., 0., 0., 0., 0., 0., 0., 0.],
+              [0., 0., 0., 0.8, 0., 0., 0., 0., 0., 0., 0., 0.],
+              [0., 0., 0., 0., 0.8, 0., 0., 0., 0., 0., 0., 0.], 
+              [0., 0., 0., 0., 0., 0.8, 0., 0., 0., 0., 0., 0.],
+              [0., 0., 0., 0., 0., 0., 0.8, 0., 0., 0., 0., 0.],
+              [0., 0., 0., 0., 0., 0., 0., 0.8, 0., 0., 0., 0.],
+              [0., 0., 0., 0., 0., 0., 0., 0., 0.8, 0., 0., 0.],
+              [0., 0., 0., 0., 0., 0., 0., 0., 0., 0.8, 0., 0.],
+              [0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.1, 0.],
+              [0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.1],]
+rollout_var_discount: 0.8
+update_cov: False
+sample_null_action: True
+filter_u: False
+use_priors: False

conf/mppi/panda_push.yaml

@@ -3,20 +3,20 @@ defaults:
 
 mppi_mode: "halton-spline"  # halton-spline, simple
 sampling_method: "halton"   # halton, random
-num_samples: 400
-horizon: 20                 # At least 12 for Halton Sampling
+num_samples: 500
+horizon: 6                # At least 12 for Halton Sampling
 device: "cuda:0"
-u_min: [-0.2]
-u_max: [0.2]
+u_min: [-0.4]
+u_max: [0.4]
 lambda_: 0.05
-noise_sigma: [[0.1, 0., 0., 0., 0., 0., 0.],
-              [0., 0.1, 0., 0., 0., 0., 0.], 
-              [0., 0., 0.1, 0., 0., 0., 0.],
-              [0., 0., 0., 0.1, 0., 0., 0.],
-              [0., 0., 0., 0., 0.1, 0., 0.],
-              [0., 0., 0., 0., 0., 0.1, 0.],
-              [0., 0., 0., 0., 0., 0., 0.1]]
-rollout_var_discount: 0.95
+noise_sigma: [[0.8, 0., 0., 0., 0., 0., 0.],
+              [0., 0.8, 0., 0., 0., 0., 0.], 
+              [0., 0., 0.8, 0., 0., 0., 0.],
+              [0., 0., 0., 0.8, 0., 0., 0.],
+              [0., 0., 0., 0., 0.8, 0., 0.],
+              [0., 0., 0., 0., 0., 0.8, 0.],
+              [0., 0., 0., 0., 0., 0., 0.8]]
+rollout_var_discount: 0.8
 update_cov: False
 sample_null_action: True
 filter_u: False