def __init__(self,learningArgs,motionModelArgs):
lr = learningArgs[0]
lrDecay_stepSize = learningArgs[1]
lrDecay_gamma = learningArgs[2]
weight_decay = learningArgs[3]
self.device = 'cuda:0' if torch.cuda.is_available() else 'cpu'
self.MotionModel = deterministicMotionModel(motionModelArgs).to(self.device)
self.optimizer = Adam(self.MotionModel.parameters(),lr = lr,weight_decay=weight_decay)
self.lrScheduler = torch.optim.lr_scheduler.StepLR(self.optimizer, step_size=lrDecay_stepSize, gamma=lrDecay_gamma)
self.criterion = torch.nn.MSELoss()
sim.resetClifford()
startState = sim.controlLoopStep([0, 0])
p.disconnect(physicsClientId=physicsClientId)
#initialize motion model
inStateDim = len(startState[0][0]) + 1
inActionDim = len(startState[0][2])
inMapDim = startState[0][1].shape[1]
outStateDim = len(startState[1][0])
argDim = [inStateDim, inMapDim, inActionDim, outStateDim]
convSizes = [[32, 5], [32, 4], [32, 3]]
fcSizes = [1024, 512, 256] #,128]
networkSizes = [convSizes, fcSizes]
dropout_ps = [0, 0, 0]
motionModelArgs = [argDim, networkSizes, dropout_ps]
motionModel = deterministicMotionModel(motionModelArgs).to(device)
motionModel.load_state_dict(
torch.load('../motionModels/v2sequentialDeterministic.pt'))
motionModel.eval()
# initialize all particles
numParticles = 10
particleStates = cliffordStateTransformation(torch.tensor(
startState[3]).unsqueeze(0).to(device),
numParticles=numParticles)
predX = [[startState[3][0]] for i in range(numParticles)]
predY = [[startState[3][1]] for i in range(numParticles)]
# loop through time and take action
for t in range(20):
# get current state of particles