diff --git a/README.md b/README.md index 1634f33..52cd6cc 100644 --- a/README.md +++ b/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:

@@ -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 ``` \ No newline at end of file diff --git a/conf/config_panda_push.yaml b/conf/config_panda_push.yaml index 08480d3..33d3225 100644 --- a/conf/config_panda_push.yaml +++ b/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]] diff --git a/conf/mppi/omnipanda.yaml b/conf/mppi/omnipanda.yaml index ab60b4a..583d149 100644 --- a/conf/mppi/omnipanda.yaml +++ b/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 diff --git a/conf/mppi/panda_push.yaml b/conf/mppi/panda_push.yaml index 75d1dd1..829e578 100644 --- a/conf/mppi/panda_push.yaml +++ b/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 diff --git a/examples/panda_push_client.py b/examples/panda_push_client.py index dc90c3a..d50a85d 100644 --- a/examples/panda_push_client.py +++ b/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 diff --git a/examples/panda_push_server.py b/examples/panda_push_server.py index 02dd9ac..6fd6887 100644 --- a/examples/panda_push_server.py +++ b/examples/panda_push_server.py @@ -60,15 +60,31 @@ def run_panda_robot(cfg: ExampleConfig): ) # Manually add table + block and restart isaacgym - obj_index = 3 + obj_index = 2 baseline = 2 - baseline_pose = 'left' + baseline2_pose = 'left' + baseline1_pose = 1 + + # Define goal to pop + if baseline == 2: + if baseline2_pose == 'left': + goal_pose = [0.7, 0.2, 0.5, 0, 0, 0.258819, 0.9659258 ] + if baseline2_pose == 'center': + goal_pose = [0.65, 0, 0.5, 0, 0, 0, 1] + if baseline2_pose == 'right': + goal_pose = [0.7, -0.2, 0.5, 0, 0, -0.258819, 0.9659258 ] + else: + if baseline1_pose == 0: + goal_pose = [0.5, 0.3, 0.5, 0.0, 0.0, 0.0, 1.0] + else: + goal_pose = [0.4, 0.3, 0.5, 0, 0, -0.7071068, 0.7071068] + # l w h mu m x y - obj_set = [[0.100, 0.100, 0.05, 0.300, 0.100, 0.37, 0.], # Baseline 1, pose 1 - [0.100, 0.100, 0.05, 0.300, 0.100, 0.40, 0.], # Baseline 1, pose 2 - [0.116, 0.116, 0.06, 0.637, 0.016, 0.35, 0.], # Baseline 2, A - [0.168, 0.237, 0.05, 0.232, 0.615, 0.38, 0.], # Baseline 2, B + obj_set = [[0.100, 0.100, 0.05, 0.200, 0.250, 0.4, 0.], # Baseline 1, pose 1 + [0.105, 0.063, 0.063, 0.20, 0.250, 0.42, 0.], # Hageslag + [0.116, 0.116, 0.06, 0.637, 0.016, 0.38, 0.], # Baseline 2, A + [0.168, 0.237, 0.05, 0.232, 0.615, 0.40, 0.], # Baseline 2, B [0.198, 0.198, 0.06, 0.198, 0.565, 0.40, 0.], # Baseline 2, C [0.166, 0.228, 0.08, 0.312, 0.587, 0.39, 0.], # Baseline 2, D [0.153, 0.462, 0.05, 0.181, 0.506, 0.37, 0.],] # Baseline 2, E @@ -78,6 +94,17 @@ def run_panda_robot(cfg: ExampleConfig): table_pos = [0.5, 0., table_dim[-1]/2] additions = [ + { + "type": "box", + "name": "obj_to_push", + "size": [obj_[0], obj_[1], obj_[2]], + "init_pos": [obj_[5], obj_[6], table_dim[-1] + obj_[2] / 2], + "mass": obj_[4], + "fixed": False, + "handle": None, + "color": [0.2, 0.2, 0.8], + "friction": obj_[3], + }, { "type": "box", "name": "table", @@ -85,11 +112,24 @@ def run_panda_robot(cfg: ExampleConfig): "init_pos": table_pos, "fixed": True, "handle": None, - "color": [0.2, 0.2, 1.0], - "noise_sigma_size": [0.005, 0.005, 0.0], - "friction": 0.2, - "noise_percentage_friction": 0.3, + "color": [255 / 255, 120 / 255, 57 / 255], + "friction": obj_[3], }, + # Add goal, + { + "type": "box", + "name": "goal", + "size": [obj_[0], obj_[1], 0.005], + "init_pos": [goal_pose[0], goal_pose[1], table_dim[-1]], + "init_ori": [goal_pose[3], goal_pose[4], goal_pose[5], goal_pose[6]], + "fixed": True, + "color": [119 / 255, 221 / 255, 119 / 255], + "handle": None, + "collision": False, + } + ] + + additions_noise = [ { "type": "box", "name": "obj_to_push", @@ -100,9 +140,33 @@ def run_panda_robot(cfg: ExampleConfig): "handle": None, "color": [0.2, 0.2, 1.0], "friction": obj_[3], - "noise_sigma_size": [0.005, 0.005, 0.0], + "noise_sigma_size": [0.002, 0.002, 0.0], "noise_percentage_friction": 0.3, "noise_percentage_mass": 0.3, + }, + { + "type": "box", + "name": "table", + "size": table_dim, + "init_pos": table_pos, + "fixed": True, + "handle": None, + "color": [0.2, 0.2, 1.0], + "noise_sigma_size": [0.005, 0.005, 0.0], + "friction": 0.25, + "noise_percentage_friction": 0.9, + }, + # Add goal, + { + "type": "box", + "name": "goal", + "size": [obj_[0], obj_[1], 0.005], + "init_pos": [goal_pose[0], goal_pose[1], table_dim[-1]], + "init_ori": [goal_pose[3], goal_pose[4], goal_pose[5], goal_pose[6]], + "fixed": True, + "color": [119 / 255, 221 / 255, 119 / 255], + "handle": None, + "collision": False, } ] @@ -112,7 +176,7 @@ def run_panda_robot(cfg: ExampleConfig): planner.connect("tcp://127.0.0.1:4242") print("Mppi server found!") - planner.add_to_env(additions) + planner.add_to_env(additions_noise) sim.gym.viewer_camera_look_at( sim.viewer, None, gymapi.Vec3(1.5, 2, 3), gymapi.Vec3(1.5, 0, 0) @@ -130,15 +194,15 @@ def run_panda_robot(cfg: ExampleConfig): count = 0 client_helper = Objective(cfg, cfg.mppi.device) init_time = time.time() - block_index = 2 + block_index = 1 data_time = [] data_err = [] - trial = 0 - timeout = 10 + n_trials = 0 + timeout = 20 rt_factor_seq = [] data_rt = [] - while trial < cfg.n_steps: + while n_trials < cfg.n_steps: t = time.time() # Reset state planner.reset_rollout_sim( @@ -169,19 +233,23 @@ def run_panda_robot(cfg: ExampleConfig): Ex, Ey, Etheta = client_helper.compute_metrics(block_pos, block_ort) metric_1 = 1.5*(Ex+Ey)+0.01*Etheta - # print("Metric Baxter", metric_1) - # print("Ex", Ex) - # print("Ey", Ey) - # print("Angle", Etheta) - # Ex < 0.025 and Ey < 0.01 and Etheta < 0.05 - # Ex < 0.05 and Ey < 0.025 and Etheta < 0.17 + # + if baseline == 1: + print("Metric Baxter", metric_1) + else: + print("Ex", Ex) + print("Ey", Ey) + print("Angle", Etheta) + # Ex < 0.025 and Ey < 0.01 and Etheta < 0.05 + # Ex < 0.05 and Ey < 0.025 and Etheta < 0.17 + # Ex < 0.02 and Ey < 0.02 and Etheta < 0.1 -- Success for baseline 1 if Ex < 0.05 and Ey < 0.025 and Etheta < 0.17: print("Success") final_time = time.time() time_taken = final_time - init_time print("Time to completion", time_taken) - reset_trial(sim, init_pos, init_vel) + reset_trial(sim, init_pos, init_vel) init_time = time.time() @@ -189,10 +257,10 @@ def run_panda_robot(cfg: ExampleConfig): data_rt.append(np.sum(rt_factor_seq) / len(rt_factor_seq)) data_time.append(time_taken) data_err.append(np.float64(metric_1)) - trial += 1 + n_trials += 1 rt_factor_seq.append(cfg.isaacgym.dt/(time.time() - t)) - # print(f"FPS: {1/(time.time() - t)} RT-factor: {cfg.isaacgym.dt/(time.time() - t)}") + print(f"FPS: {1/(time.time() - t)} RT-factor: {cfg.isaacgym.dt/(time.time() - t)}") count = 0 else: @@ -202,10 +270,7 @@ def run_panda_robot(cfg: ExampleConfig): reset_trial(sim, init_pos, init_vel) init_time = time.time() count = 0 - data_time.append(-1) - data_err.append(-1) - data_rt.append(-1) - trial += 1 + n_trials += 1 # Visualize samples # rollouts = bytes_to_torch(planner.get_rollouts()) @@ -215,20 +280,36 @@ def run_panda_robot(cfg: ExampleConfig): # pos = sim.root_state[0, -1][:2].cpu().numpy() # goal = np.array([0.5, 0]) # print(f"L2: {np.linalg.norm(pos - goal)} FPS: {1/(time.time() - t)} RT-factor: {cfg.isaacgym.dt/(time.time() - t)}") + + # To array + data_time = np.array(data_time) + data_rt = np.array(data_rt) + actual_time = data_time*data_rt + data_err = np.array(data_err) + + if len(data_time) > 0: + print("Num. trials", n_trials) + print("Success rate", len(data_time)/n_trials*100) + print("Avg. Time", np.mean(actual_time)) + print("Std. Time", np.std(actual_time)) + if baseline == 1: + print("Avg. error", np.mean(data_err)) + print("Std. error", np.std(data_err)) + else: + print("Seccess rate is 0") # Print for data collection # original_stdout = sys.stdout # Save a reference to the original standard output - + # obj_letters = ["A", "B", "C", "D", "E"] # with open('data_push_baselines.txt', 'a') as f: # sys.stdout = f # Change the standard output to the file we created. - # print('Benchmark object {} in baseline {} for pose "{}"'.format(obj_index-2, baseline, baseline_pose)) - # print('Time taken:', np.around(data_time, decimals=3)) - # print('Placement error:', np.around(data_err, decimals=3)) - # print('RT factor:', np.around(data_rt, decimals=3)) + # print('Benchmark object {} in baseline {} for pose "{}"'.format(obj_letters[obj_index-2], baseline, baseline2_pose)) + # print("Num. trials", n_trials) + # print("Num. success", len(data_err)) + # print('Time taken:', repr(actual_time)) # print('\n') # sys.stdout = original_stdout # Reset the standard output to its original value - return {}