Merging RL changes into main

.gitignore

@@ -133,4 +133,21 @@ mojograsp/simcore/simmanager/data/*
 demos/full_rl_demo/data/*
 demos/full_rl_demo/agent_replay*
 demos/full_rl_demo/expert_replay*
-demos/full_rl_demo/*weights*
+demos/full_rl_demo/*weights*
+
+# saved data
+
+demos/rl_demo/data/*
+demos/expert_demo/data/*
+demos/rl_demo/runs/*
+demos/rl_demo/old_runs/*
+*pickapp
+*.pkl
+*.png
+*.prof
+*.zip
+tests/runs/*
+tests/no/*
+mojograsp/simcore/wandb/*
+demos/rl_demo/wandb/*
+distance_and_efficiency.csv

aout.csv

@@ -0,0 +1,152 @@
+a1,a2,a3,a4
+0.9963051884917578,0.9257133381522447,0.998655994598375,-0.032275044541869274
+0.9975944790601811,0.9249961390574316,0.9983908780820573,-0.03754597414095302
+0.9983904985241631,0.9219761498678325,0.9997581078961628,-0.04935616569200349
+0.9964217269853433,0.911808085457898,1.0,-0.07342940941707804
+0.9965156991667193,0.8913254998520939,0.9989989783539759,-0.0879005279408016
+0.995649153933241,0.8651916428051285,0.9982264607103384,-0.09612611261331312
+0.9908744102462439,0.8265485480774067,0.9982040058604749,-0.0891190161178737
+0.9870293997224358,0.7776570081077003,0.9987553948650997,-0.07689886317658663
+0.980978141750486,0.7188240732100004,1.0,-0.06888674131876049
+0.9698409824090651,0.6476092269096219,1.0,-0.06802075327528487
+0.9541814875647134,0.5638831727865758,1.0,-0.06519955097997067
+0.927597033861995,0.47086302991292606,0.999551617755383,-0.052352332626572
+0.8855523533904113,0.3568754559182657,0.9978077266529709,-0.043595931096778395
+0.8253020810061253,0.2316375843413056,0.9985361031471471,-0.0359123761001931
+0.7400369578680399,0.10476726117731351,0.9997532197988417,-0.026452673222880623
+0.6321172409812374,-0.02385692405168034,0.9974964825472928,-0.014727445931275224
+0.5095659831198003,-0.1397469524527608,0.9990477784072912,-0.00617099942154236
+0.37838987284374886,-0.25099161550494153,0.9979209684398846,0.007572760372142851
+0.2497732330366996,-0.34882233042596916,0.9981451974146066,0.019354169453688275
+0.13819135561543514,-0.4252126032879047,0.9981942506735991,0.019567672615662127
+0.05111725598590585,-0.47112887099817796,0.9971510902652563,0.025657660262425542
+-0.012417346572548213,-0.5000558943924583,0.997207023647835,0.04027040557290095
+-0.07093762156248566,-0.5238205814125146,0.9947421457786078,0.04586222742651252
+-0.1068757119867672,-0.5377237916807669,0.995097971475839,0.04816843762567475
+-0.1309765123817956,-0.540294746803794,0.9942936102038116,0.05299251139524605
+-0.14544215771828234,-0.5355037631481032,0.9924157883740344,0.06326442005138726
+-0.15601892137724513,-0.5463022677074265,0.9900553436857564,0.08536683593961608
+-0.1899986551289402,-0.5773280499747361,0.9890866781707994,0.11307419354690094
+-0.24768594640372474,-0.619926941895652,0.986050174972904,0.13911287812183654
+-0.29652006867493425,-0.6636930306371991,0.9844974334907611,0.17369281705212722
+-0.3541500347540797,-0.7095294358866465,0.9810092499444676,0.20698432293862173
+-0.4132405018798677,-0.7498447538368045,0.9781387129076992,0.23243265650672743
+-0.47128546784336844,-0.7814416588908095,0.9723483767383342,0.2517449580239239
+-0.5263544798739156,-0.8122300318436566,0.9679205808140691,0.2651700698805002
+-0.5862954994190739,-0.8432832993119916,0.9628048089651643,0.2945346129671771
+-0.6422646149716228,-0.872643694447921,0.9575242108782483,0.32809190963489326
+-0.6971026667916911,-0.8926474095881316,0.9497364483783768,0.3738850253794468
+-0.7418361573491218,-0.913191071163624,0.9420275417983716,0.40798316562967135
+-0.7763306293562799,-0.9297925734635423,0.9353188088442702,0.44092353579734217
+-0.8027003889867248,-0.943885443176469,0.9286985881323961,0.469905605357378
+-0.8241110172230092,-0.9531237413994202,0.9214663351170893,0.497316592354248
+-0.8436839102539762,-0.9618760001399438,0.913483501055455,0.5324169289534133
+-0.8605753651854133,-0.9659671007263336,0.9042681343699986,0.5694289050541235
+-0.8698992770970803,-0.9718470674874048,0.8957004600766972,0.6040416684488711
+-0.8741757596148148,-0.9757114977919338,0.8851128746908153,0.6358353109691274
+-0.8770994643926899,-0.9785339853278431,0.8775565995017359,0.6666655775008131
+-0.878852129390953,-0.9796702839433363,0.8707509113491332,0.6903859826462171
+-0.8769991746848389,-0.9819400683108398,0.8627486283736339,0.7114262364355954
+-0.8781373566253521,-0.9821368434221938,0.8552382776902471,0.7237192913686648
+-0.8729759373183846,-0.9836252188230281,0.8480452400436317,0.740828704098343
+-0.8641892620172454,-0.9835944533521876,0.8403701929887362,0.7577303157800265
+-0.855436452451907,-0.9854227818428962,0.8316731924928308,0.769731341311243
+-0.8461227121852938,-0.9850937253294112,0.8233889208804016,0.777323362050591
+-0.8341436865913952,-0.9857057909345796,0.8115358809932547,0.7820147089521446
+-0.8236943522053212,-0.9857055158930826,0.7982050582916325,0.7863549901425898
+-0.8187142138693857,-0.9871666145201831,0.7823246883669981,0.7879072232747615
+-0.8102053160367525,-0.9882547682767722,0.7644068377010872,0.7974653820000541
+-0.8053975427110264,-0.9887441674541299,0.7427353439799599,0.8121745523601881
+-0.80192871521137,-0.9889909104578736,0.7226850807339993,0.8260090964195099
+-0.7945441095851058,-0.9890395807684789,0.7031274036284944,0.8397312677249045
+-0.7888176183845199,-0.9898876475106159,0.6902984818939287,0.8511623225001591
+-0.7826775650440674,-0.990270678802715,0.6699799997396914,0.8627007914719239
+-0.778797784249122,-0.9913856631363229,0.6558378955798067,0.875412240266854
+-0.7751420456792365,-0.9924705590836691,0.6405262725839809,0.8864303707981687
+-0.7791770617923202,-0.9931336085749012,0.6234648263335714,0.8933330356901577
+-0.7771914814527645,-0.9953029630719942,0.6083443344499341,0.9000979218688118
+-0.7733159452361049,-0.9950491631239754,0.5931552833562661,0.9069694384047006
+-0.7737070911829496,-0.9968228949100536,0.5738970652766332,0.9150630948310124
+-0.7722278926404152,-0.9965296165168911,0.5568030025844889,0.920376448480353
+-0.7749858720221331,-0.9975435400007762,0.5482928237820258,0.9257448989328282
+-0.7747348443173988,-0.9978619648431218,0.5340007565991471,0.9349173870180474
+-0.7793620910674612,-0.9978783228970665,0.5257216191766336,0.9406494160997121
+-0.7803694450234013,-0.9990872273899076,0.5169938565747116,0.9461250325717034
+-0.7846683706175561,-0.9976412278222014,0.5278133121000728,0.9492200691023138
+-0.7861363773122181,-0.9989793948455256,0.5304648545119384,0.9503887017214191
+-0.7778533314577036,-0.997678106976718,0.5419967531854086,0.9528193978832387
+-0.7670220041273582,-0.9977449918842433,0.5544333781477787,0.9557040827893648
+-0.7494011909209901,-0.9993787255877116,0.569165061883493,0.9585932944148343
+-0.7234367498480848,-0.9989991239433368,0.5873865901815127,0.9591363060253365
+-0.6907807716246551,-0.9986398539383085,0.6105374448595116,0.9582827634678466
+-0.6474708773581541,-0.9996759885702824,0.6345253308240029,0.9579506678539849
+-0.5955432787770695,-0.9989114056331843,0.652344054225189,0.957243285201948
+-0.5398490920929732,-1.0,0.6555722839696616,0.9560958186616055
+-0.46990684154164897,-1.0,0.6592365513920326,0.9560136632167632
+-0.37915724973905407,-0.9996873159950607,0.6522670544899144,0.9548512588077386
+-0.28232196458023584,-0.9997808964958872,0.6369318218505555,0.9529241230027179
+-0.17169016082899508,-0.999270635919995,0.6211477150089976,0.9513703786788035
+-0.05242198428840689,-1.0,0.6060758739232197,0.9507020274145033
+0.07030208724143956,-1.0,0.5772303669415688,0.9493185488338205
+0.16282759694732243,-0.9996829814509568,0.5449841235637791,0.9477748841156313
+0.24017441559254638,-0.9994187560553791,0.5205547137800884,0.9477741710863491
+0.2947016773088778,-0.9993660741064984,0.4978940653322345,0.9467731057768823
+0.3274407387931778,-1.0,0.4749438701165818,0.9442727884061315
+0.3445184630372652,-0.9988076322256831,0.44853073503047397,0.9420757690800647
+0.3611926105065727,-0.9998508065464071,0.423368328632533,0.9374220425821106
+0.3767631114815551,-0.9986458401691094,0.39975650806343044,0.933723494900849
+0.383886233144451,-0.9989545943223019,0.37882169620017664,0.9297841723537639
+0.38053684565021056,-0.9993205867243161,0.3558742003139988,0.9238914978181844
+0.36563330829961943,-0.9999018991942765,0.3304703933209151,0.9211879650875391
+0.3348435006730547,-1.0,0.306965785122508,0.9184079858162317
+0.2883237004760395,-0.9995069886748981,0.28492076254712023,0.9180797223270555
+0.23944611851705747,-1.0,0.27415650031654615,0.9154714015506353
+0.18856330365343835,-0.999016632611481,0.26243365028791654,0.9142821732270777
+0.1317396044833481,-0.9998175578661462,0.2390158110617888,0.9171606408732741
+0.06088627451944107,-1.0,0.2080228764465495,0.9197394465526575
+-0.012134110419618283,-1.0,0.15711791584730161,0.9228625341705092
+-0.08940406691122571,-0.9994510739127895,0.10304297989422648,0.9219798800655546
+-0.16744118855777357,-0.999297444622946,0.04753672311587086,0.9221540548090553
+-0.2344847535742474,-1.0,-0.001198912067023198,0.9251906954821738
+-0.29952876598185296,-1.0,-0.05649260297656587,0.927052355774135
+-0.3742509015562776,-0.9999478912071057,-0.11195805585446766,0.9284183951018388
+-0.44247483254816694,-0.9991435546220754,-0.15865755785693164,0.930290597252881
+-0.500290867377605,-0.999953525712564,-0.19627664607851164,0.9322523850748324
+-0.5437922673451392,-1.0,-0.22150666122707194,0.9309128450363866
+-0.577513295427487,-0.9991466694597059,-0.23397750141064186,0.9317804075543297
+-0.6022688675266386,-0.9999988914738078,-0.23892791466461163,0.9321141239990671
+-0.623597213287012,-1.0,-0.23044208573015912,0.9298077738229037
+-0.6345770762226945,-0.9992076226925737,-0.2182023881319699,0.9279361583664003
+-0.6377007015208603,-0.9998547117906906,-0.1903551472827123,0.9230789138215453
+-0.6422639462258629,-1.0,-0.15585707331339393,0.9122149716552694
+-0.6382472692511473,-0.9998587389073269,-0.10811812873834663,0.9016876081639505
+-0.6228545460112357,-1.0,-0.05674190409771237,0.8899294881537869
+-0.5986635007298646,-1.0,0.013604327677423016,0.8768724416733121
+-0.5775736129586821,-0.9995221011889559,0.08475434932204001,0.8662716292955294
+-0.5525550647469377,-0.9996639808677222,0.1480611925377892,0.8544114106894537
+-0.5339843115711879,-1.0,0.19223051511382463,0.8448746520816814
+-0.5281119518077948,-1.0,0.2272503944209179,0.8350571413237315
+-0.5207600690614395,-0.999493927388866,0.24923548933207865,0.8257794084020564
+-0.5168412959866994,-0.9988611351243193,0.26865341217027333,0.8225943361586833
+-0.5115436416405399,-0.9990119843749639,0.2817972331834916,0.8230247558513472
+-0.5098641081557678,-0.9998387729671182,0.29168771633203666,0.8253600730762538
+-0.504932150969371,-1.0,0.3053517848539571,0.824944227729239
+-0.4994211671480967,-0.9991710819668681,0.3101865079932703,0.8269269883578214
+-0.4920775821910381,-1.0,0.3145952335138202,0.8288842034628907
+-0.48711048487825565,-1.0,0.30979254876080536,0.8357518967539024
+-0.48107781950136375,-1.0,0.29920380536155067,0.8466907508029949
+-0.47357193490994687,-0.9998472151388422,0.27110986089150946,0.8588591712289676
+-0.4623325733950438,-0.9998870854452548,0.24241150818657162,0.8671251542201242
+-0.45825371524156333,-1.0,0.1984561541053008,0.8754071697218365
+-0.4614458134676032,-1.0,0.1559637596263994,0.8804093604101692
+-0.46259594433049833,-1.0,0.12052583111655961,0.8861004763515938
+-0.43918443362801873,-0.9999035259737524,0.10619710924511248,0.89047417362148
+-0.41342934075827203,-0.999325330895236,0.10473578037075584,0.8950628151689877
+-0.3900300927980956,-1.0,0.11711350482147978,0.8984276596888094
+-0.3818833276916024,-0.9991236693861562,0.1398800595257331,0.9011826120079378
+-0.3774342131827225,-0.9999938312718082,0.18546516765427215,0.9076696445894069
+-0.3746059623120426,-0.9998733579291965,0.2608439448385691,0.9088080762002506
+-0.3697122342011157,-1.0,0.347275569824407,0.9086346496880854
+-0.35411434747841336,-0.9998223089138412,0.4317834080296131,0.9079217549857542
+-0.349721873348731,-1.0,0.5133994496664414,0.904154905759215
+-0.3448617337673327,-1.0,0.5888332815796695,0.9016283580174144

build_episode.py

@@ -0,0 +1,36 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+"""
+Created on Thu Jun  1 08:53:49 2023
+
+@author: orochi
+"""
+
+
+
+import os
+import re
+import pickle as pkl
+
+folder = '/home/orochi/mojo/mojo-grasp/demos/real_world/'
+episode_files = [os.path.join(folder, f) for f in os.listdir(folder) if f.lower().endswith('.pkl')]
+filenames_only = [f for f in os.listdir(folder) if f.lower().endswith('.pkl')]
+real_ones = [f for f in filenames_only if f[0].isupper()]
+print(filenames_only)
+print(real_ones)
+for filename in real_ones:
+    direction = re.findall('\w+',filename)
+    print(direction[0])
+    # a = [f for f in filenames_only if f.startswith(direction[0])]
+    with open(folder+direction[0]+'.pkl', 'rb') as file:
+        state = pkl.load(file)
+    # with open(folder+direction[0]+'actor.pkl', 'rb') as file:
+    #     action = pkl.load(file)
+    edict = {'number': 0, 'timestep_list':[]}
+    # for i,j in zip(state,action):
+    #     edict['timestep_list'].append({'state':i,'action':j})
+    for i in state:
+        edict['timestep_list'].append({'state':i})
+
+    with open(folder+direction[0] +'episode.pkl','wb') as file:
+        pkl.dump(edict,file)

config_rewriter.py

@@ -0,0 +1,68 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+"""
+Created on Thu Apr 27 13:55:03 2023
+
+@author: orochi
+"""
+import json
+import os
+import pathlib
+
+folder_path = './demos/rl_demo/data/HPC_Full'
+
+overall_path = pathlib.Path(__file__).parent.resolve()
+resource_path = overall_path.joinpath('demos/rl_demo/resources')
+run_path = overall_path.joinpath('demos/rl_demo/runs')
+batch_run_folder = overall_path.joinpath(folder_path)
+
+subfolders = os.listdir(batch_run_folder)
+
+if 'experiment_config.json' in subfolders:
+    print('Folder path is a single configuration')
+
+    curr_folder = str(batch_run_folder)
+    with open(curr_folder+'/experiment_config.json', 'r') as file:
+        config = json.load(file)
+    high_level_path =  pathlib.Path(config['save_path']).parent.resolve().parent.resolve().parent.resolve().parent.resolve()#.parent.resolve()
+    high_level_path = str(high_level_path)
+    print('hlp', high_level_path)
+    print('overall folder', str(overall_path))
+    print('overall path', )
+    for k in config.keys():
+        if type(config[k]) == str:
+            print('old',k,config[k])
+            config[k] = config[k].replace(high_level_path, str(overall_path))
+            print('new',k,config[k])
+        elif type(config[k]) == list:
+            for i in range(len(config[k])):
+                if type(config[k][i])==str:
+                    print('old',k,config[k][i])
+                    config[k][i] = config[k][i].replace(high_level_path, str(overall_path))
+                    print('new',k,config[k][i])
+    with open(curr_folder+'/experiment_config.json', 'w') as file:
+        json.dump(config,file)
+else:
+    print('Folder path is a folder with subfolders')
+    for folder in subfolders:
+        curr_folder = str(batch_run_folder.joinpath(folder))
+        with open(curr_folder+'/experiment_config.json', 'r') as file:
+            config = json.load(file)
+        high_level_path =  pathlib.Path(config['save_path']).parent.resolve().parent.resolve().parent.resolve().parent.resolve().parent.resolve()
+        high_level_path = str(high_level_path)
+        print('hlp', high_level_path)
+        print('overall folder', str(overall_path))
+        for k in config.keys():
+            if type(config[k]) == str:
+                print('old',k,config[k])
+                config[k] = config[k].replace(high_level_path, str(overall_path))
+                print('new',k,config[k])
+            elif type(config[k]) == list:
+                for i in range(len(config[k])):
+                    if type(config[k][i])==str:
+                        print('old',k,config[k][i])
+                        config[k][i] = config[k][i].replace(high_level_path, str(overall_path))
+                        print('new',k,config[k][i])
+
+        with open(curr_folder+'/experiment_config.json', 'w') as file:
+            json.dump(config,file)

demos/cheetah/main.py

@@ -0,0 +1,59 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+"""
+Created on Thu Sep 29 14:44:46 2022
+
+@author: orochi
+"""
+import pybullet as p
+import time
+import pybullet_data
+from multiprocessing import connection
+from operator import truediv
+import pybullet as p
+import pybullet_data
+import pathlib
+import manipulation_phase_rl
+import rl_env
+from rl_state import StateRL, GoalHolder
+import rl_action
+import rl_reward
+import pandas as pd
+from mojograsp.simcore.sim_manager import SimManagerRL
+from mojograsp.simcore.state import StateDefault
+from mojograsp.simcore.reward import RewardDefault
+from mojograsp.simcore.record_data import RecordDataJSON, RecordDataPKL,  RecordDataRLPKL
+from mojograsp.simobjects.two_finger_gripper import TwoFingerGripper
+from mojograsp.simobjects.object_base import ObjectBase
+from mojograsp.simobjects.object_with_velocity import ObjectWithVelocity
+from mojograsp.simobjects.object_for_dataframe import ObjectVelocityDF
+from mojograsp.simcore.replay_buffer import ReplayBufferDefault, ReplayBufferDF
+import numpy as np
+physicsClient = p.connect(p.GUI)
+p.setAdditionalSearchPath(pybullet_data.getDataPath())
+p.setGravity(0,0,-10)
+planeId = p.loadURDF("plane.urdf")
+startPos = [0,0,1]
+startOrientation = p.getQuaternionFromEuler([0,0,0])
+# cheetahId = p.loadURDF("/minic_cheetah/mini_cheetah.urdf", startPos, startOrientation)
+# cheetahId = p.loadURDF("./mini_cheetah/mini_cheetah.urdf", startPos, startOrientation)
+cheetahId = p.loadMJCF('./mjcf/half_cheetah.xml')
+
+state = StateRL(objects=[cheetahId])
+action = rl_action.ExpertAction()
+reward = rl_reward.ExpertReward()
+arg_dict = {'state_dim': 14, 'action_dim': 4, 'max_action': 1.57, 'n': 5, 'discount': 0.995, 'tau': 0.0005,
+            'batch_size': 100, 'expert_sampling_proportion': 0.7}
+
+
+# replay buffer
+replay_buffer = ReplayBufferDefault(buffer_size=400000, state=state, action=action, reward=reward)
+# replay_buffer = ReplayBufferDF(state=state, action=action, reward=reward)
+
+# Run the sim
+done_training = False
+training_length = 100
+while not done_training:
+    for k in range(training_length):
+
+p.disconnect()

demos/cheetah/rl_action.py

@@ -0,0 +1,12 @@
+from mojograsp.simcore.action import Action
+
+
+class ExpertAction(Action):
+    def __init__(self):
+        self.current_action = {}
+
+    def set_action(self, joint_angles: list):
+        self.current_action["target_joint_angles"] = joint_angles
+
+    def get_action(self) -> dict:
+        return self.current_action.copy()