Merge pull request #43 from tud-airlab/ft-tuning-panda-push

Ft tuning panda push

README.md

@@ -1,5 +1,5 @@
 # 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: 
+This is an implementation of a Model Predictive Path Integral (MPPI) controller which uses IsaacGym as dynamic 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"/>
@@ -20,7 +20,7 @@ You can install the necesary dependancies using [poetry](https://python-poetry.o
 ```bash
 poetry install --with dev
 ```
-Bare in mind that the installation might take several minutes the first time. But it's worth it.
+Bear in mind that the installation might take several minutes the first time. But it's worth it.
 
 Access the virtual environment using
 ```bash
@@ -34,16 +34,16 @@ 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:
+Alternatively, you can also install at the system level using pip, even though we advise using the virtual environment:
 ```bash
 pip install .
 ```
 
 ## 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
+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 for every terminal
 
 # 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. 
+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:
@@ -56,7 +56,7 @@ python3 heijn_push_client.py
 ```
 
 ## Pybullet examples
-Simply run one of the example scripts which use pybullet, for instance for the panda robot:
+Simply run one of the example scripts which use Pybullet, for instance for the panda robot:
 ```bash
 python3 panda_robot_with_obstacles.py
 ```

conf/config_panda_push.yaml

@@ -4,7 +4,7 @@ defaults:
 
 goal: [0.5, 0.4, 0.7]
 render: true
-n_steps: 1000
+n_steps: 10
 actors: ['panda_stick']
 initial_actor_positions: [[0.0, 0.0, 0.0]]
 

conf/mppi/omnipanda.yaml

@@ -21,7 +21,7 @@ noise_sigma: [[0.5, 0., 0., 0., 0., 0., 0., 0., 0., 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
+rollout_var_discount: 0.95
 update_cov: False
 sample_null_action: True
 filter_u: False

conf/mppi/panda_push.yaml

@@ -4,10 +4,10 @@ defaults:
 mppi_mode: "halton-spline"  # halton-spline, simple
 sampling_method: "halton"   # halton, random
 num_samples: 500
-horizon: 6                # At least 12 for Halton Sampling
+horizon: 8                # At least 12 for Halton Sampling
 device: "cuda:0"
-u_min: [-0.4]
-u_max: [0.4]
+u_min: [-0.35]
+u_max: [0.35]
 lambda_: 0.05
 noise_sigma: [[0.8, 0., 0., 0., 0., 0., 0.],
               [0., 0.8, 0., 0., 0., 0., 0.], 
@@ -16,7 +16,7 @@ noise_sigma: [[0.8, 0., 0., 0., 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
+rollout_var_discount: 0.95
 update_cov: False
 sample_null_action: True
 filter_u: False

examples/panda_push_client.py

@@ -17,25 +17,28 @@
 class Objective(object):
     def __init__(self, cfg, device):
 
-        # Tuning of the weights for baseline 2
-        self.w_robot_to_block_pos= 1#1
-        self.w_block_to_goal_pos=  20#5
-        self.w_block_to_goal_ort=  20#5
-        self.w_ee_hover=           55#15
-        self.w_ee_align=           .3#0.5
-        self.w_push_align=         45#0.5
-
-        # Tuning of the weights for baseline 1 nd eal experiments
-        # self.w_robot_to_block_pos= 1#0.5
-        # self.w_block_to_goal_pos=  8#8.0 
+        # Tuning of the weights for baseline 1 
+        # self.w_robot_to_block_pos= 1#2
+        # self.w_block_to_goal_pos=  16#6.0 
         # self.w_block_to_goal_ort=  2#2.0
-        # self.w_ee_hover=           15#5
+        # self.w_ee_hover=           8#5
         # self.w_ee_align=           0.5#0.5
-        # self.w_push_align=         0.5#1.0
+        # self.w_push_align=         0.8#0.4
+        # self.w_collision=          0.0
+
+        # Tuning of the weights for baseline 2
+        self.w_robot_to_block_pos= 5#2
+        self.w_block_to_goal_pos=  25#12.0 
+        self.w_block_to_goal_ort=  21#10.0
+        self.w_ee_hover=           30#5
+        self.w_ee_align=           .3#0.2
+        self.w_push_align=         45#4.2
+        self.w_collision=          0.0
 
+
         # Task configration for comparison with baselines
         self.ee_index = 9
-        self.block_index = 2
+        self.block_index = 1
         self.ort_goal_euler = torch.tensor([0, 0, 0], device=cfg.mppi.device)
         self.ee_hover_height = 0.14
 
@@ -47,13 +50,14 @@ def __init__(self, cfg, device):
         self.block_goal_pose_ur5_r= torch.tensor([0.7, -0.2, 0.5,  0, 0, -0.258819, 0.9659258 ], device=cfg.mppi.device) # Rotation -30 deg
 
         # Select goal according to test
-        self.block_goal_pose = torch.clone(self.block_goal_pose_ur5_r)
+        self.block_goal_pose = torch.clone(self.block_goal_pose_ur5_l)
         self.block_ort_goal = torch.clone(self.block_goal_pose[3:7])
         self.goal_yaw = torch.atan2(2.0 * (self.block_ort_goal[-1] * self.block_ort_goal[2] + self.block_ort_goal[0] * self.block_ort_goal[1]), self.block_ort_goal[-1] * self.block_ort_goal[-1] + self.block_ort_goal[0] * self.block_ort_goal[0] - self.block_ort_goal[1] * self.block_ort_goal[1] - self.block_ort_goal[2] * self.block_ort_goal[2])
 
         self.success = False
         self.ee_celebration = 0.25
         self.count = 0
+        self.obst_index = 11
 
     def compute_metrics(self, block_pos, block_ort):
 
@@ -70,8 +74,6 @@ def compute_cost(self, sim):
         block_pos = sim.root_state[:, self.block_index, :3]
         block_ort = sim.root_state[:, self.block_index, 3:7]
 
-        # print(block_pos[-1])
-
         # Distances robot
         robot_to_block = r_pos - block_pos
         robot_euler = pytorch3d.transforms.matrix_to_euler_angles(pytorch3d.transforms.quaternion_to_matrix(r_ort), "ZYX")   
@@ -94,15 +96,20 @@ def compute_cost(self, sim):
         push_align = torch.sum(robot_to_block[:,0:2]*block_to_goal, 1)/(robot_to_block_dist*block_to_goal_pos)+1
 
         # print(push_align[-1])
-
+        # Collision avoidance
+        xyz_contatcs = torch.reshape((torch.sum(torch.abs(sim.net_cf), dim=1)), (sim.num_envs, sim.num_bodies))
+        # print(len(xy_contatcs.reshape([sim.num_envs, int(xy_contatcs.size(dim=0)/sim.num_envs)])))
+        # coll = torch.sum(xy_contatcs.reshape([sim.num_envs, int(xy_contatcs.size(dim=0)/sim.num_envs)])[:, (sim.num_bodies - self.obst_number):sim.num_bodies], 1)
+        coll = xyz_contatcs[:,self.obst_index]
 
         total_cost = (
-            self.w_robot_to_block_pos * robot_to_block_dist
+            self.w_robot_to_block_pos * robot_to_block_dist 
             + self.w_block_to_goal_pos * block_to_goal_pos
             + self.w_block_to_goal_ort * block_to_goal_ort
             + self.w_ee_hover * ee_hover_dist
             + self.w_ee_align * ee_align
             + self.w_push_align * push_align
+            + self.w_collision * coll
         )
 
         return total_cost