def theInitializer(istate, desTraj):
#! Controller needs building.
cfS.controller.tracker(desTraj)
#! simController configuration should be instantiated.
theSim = simController(solver, cfS.controller)
theSim.initializeByStruct(desTraj.tspan, istate)
def setup(self,
numTrajectoriesPerSim,
trajectoryLength,
simulationParamsIn={},
cliffordParamsIn={},
terrainMapParamsIn={},
terrainParamsIn={},
senseParamsIn={}):
# start sim
physicsClientId = p.connect(p.DIRECT)
# initialize clifford robot
robot = Clifford(params=cliffordParams,
physicsClientId=physicsClientId)
# initialize simulation controller
self.sim = simController(robot,
simulationParamsIn=simulationParamsIn,
senseParamsIn=senseParamsIn,
terrainMapParamsIn=terrainMapParamsIn,
terrainParamsIn=terrainParamsIn,
physicsClientId=physicsClientId)
data = self.sim.controlLoopStep([0, 0])
bufferLength = numTrajectoriesPerSim * trajectoryLength
self.replayBuffer = sequentialReplayBuffer(bufferLength, data[0],
data[1])
self.trajectoryLength = trajectoryLength
self.numTrajectoriesPerSim = numTrajectoriesPerSim
def setup(self,
numTrajectoriesPerSim,
trajectoryLength,
root_dir,
simulationParamsIn={},
cliffordParamsIn={},
terrainMapParamsIn={},
terrainParamsIn={},
senseParamsIn={}):
print("setup sim " + str(self.index))
# save parameters
self.trajectoryLength = trajectoryLength
self.numTrajectoriesPerSim = numTrajectoriesPerSim
self.root_dir = root_dir
# start sim
physicsClientId = p.connect(p.DIRECT)
# initialize clifford robot
robot = Clifford(params=cliffordParamsIn,
physicsClientId=physicsClientId)
# initialize simulation controller
self.sim = simController(robot,
simulationParamsIn=simulationParamsIn,
senseParamsIn=senseParamsIn,
terrainMapParamsIn=terrainMapParamsIn,
terrainParamsIn=terrainParamsIn,
physicsClientId=physicsClientId)
stateAction, newState, terminateFlag = self.sim.controlLoopStep([0, 0])
self.fileCounter = 0
self.filenames = []
self.trajectoryLengths = []
def linSysBuilder(istate, desTraj):
ctrl = theControl.tracker(desTraj.x, desTraj.u, desTraj.statedep)
if theControl.K.shape[1] == 2 * istate.x.size:
istate.x = np.pad(istate.x.flatten(), (0, istate.x.size),
mode='constant').reshape(
(theControl.K.shape[1], 1))
csim = simController(solver, ctrl)
csim.initialize(tspan=desTraj.tspan, x0=istate.x)
return csim
def linSysBuilder(istate, fstate):
if 'cons' in sys:
(ctrl,
xdes) = theControl.regulatorConstrained(fstate, sys.cons)
else:
(ctrl, xdes) = theControl.regulator(fstate)
if theControl.K.shape[1] == 2 * istate.size:
istate = np.pad(istate.flatten(), (0, istate.size),
mode='constant').reshape(
(theControl.K.shape[1], 1))
csim = simController(solver, ctrl)
csim.initialize(tspan=sys.tspan, x0=istate)
return csim
import sys
sys.path.append("..")
import pybullet as p
from simController import simController
import matplotlib.pyplot as plt
import torch
import numpy as np
from cliffordStateTransformation import cliffordStateTransformation
from motionModel import deterministicMotionModel
device = 'cuda:0' if torch.cuda.is_available() else 'cpu'
# start a sim to generate starting state and random terrain
physicsClientId = p.connect(p.DIRECT)
sim = simController(timeStep=1. / 500.,
stepsPerControlLoop=50,
numSolverIterations=300,
physicsClientId=physicsClientId)
#sim.terrain.generate(AverageAreaPerCell = 1,cellPerlinScale=5,cellHeightScale=0.75)
sim.terrain.generate(AverageAreaPerCell=1,
cellPerlinScale=5,
cellHeightScale=0.6,
smoothing=1)
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])
e1 = np.array([2, 5, -6]).reshape((3, 1))
e1 = e1 / np.linalg.norm(e1)
e2 = np.array([0, 6, 5]).reshape((3, 1))
e2 = e2 / np.linalg.norm(e2)
e3 = np.cross(np.squeeze(e1), np.squeeze(e2))
gd = theGroup(x=np.zeros((3, 1)), R=np.hstack((e1, e2, e3[:, np.newaxis])))
elif (initFlag == 2):
e1 = np.array([2, 5, -6]).reshape((3, 1))
e1 = e1 / np.linalg.norm(e1)
e2 = np.array([0, 6, 5]).reshape((3, 1))
e2 = e2 / np.linalg.norm(e2)
e3 = np.cross(np.squeeze(e1), np.squeeze(e2))
gd = theGroup(x=2 * np.ones((3, 1)),
R=np.hstack((e1, e2, e3[:, np.newaxis])))
print(gd)
theSim = simController(theSolver, theController.regulator(gd)[0])
theSim.initialize(tSpan, g0)
sol = theSim.simulate()
print(sol.x[0])
print(sol.x[-1])
plt.figure()
ax = plt.axes(projection='3d')
for i in range(len(sol.t)):
sol.x[i].plot(ax=ax, scale=0.2)
#SE3().plot(ax=ax, scale=0.2)
ax.set_box_aspect(
[ub - lb for lb, ub in (getattr(ax, f'get_{a}lim')() for a in 'xyz')])
plt.show()
sim.terrain.generate()
sim.resetClifford()
return collectedData
if __name__ == "__main__":
replayBufferLength = 500000
numParallelSims = 16
sims = []
# set up simulations
for i in range(numParallelSims):
if i == -1:
physicsClientId = p.connect(p.GUI)
else:
physicsClientId = p.connect(p.DIRECT)
sims.append(simController(physicsClientId=physicsClientId))
data = sims[0].controlLoopStep([0, 0])
replayBuffer = ReplayBuffer(replayBufferLength,
data[0],
data[1],
saveDataPrefix='simData/',
chooseCPU=True)
sTime = time.time()
executor = concurrent.futures.ProcessPoolExecutor()
while not replayBuffer.bufferFilled:
results = executor.map(runSim, sims)
for result in results:
for data in result:
replayBuffer.addData(data[0], data[1])
import pybullet as p
from simController import simController
from motionModel import deterministicMotionModel
import matplotlib.pyplot as plt
import torch
import numpy as np
#from standardizeData import standardizeData
from cliffordStateTransformation import cliffordStateTransformation
device = 'cuda:0' if torch.cuda.is_available() else 'cpu'
physicsClientId = p.connect(p.GUI)
sim = simController(physicsClientId=physicsClientId)
startState = sim.controlLoopStep([0, 0])
cliffordStatePred = cliffordStateTransformation(
torch.tensor(startState[3]).unsqueeze(0).to(device))
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/deterministic.pt'))
motionModel.load_state_dict(
torch.load('motionModels/sequentialDeterministic.pt'))
rsig = lambda t: np.array([[ra * np.sin(rw * t)], [(ra * rw) * np.cos(rw * t)]]
)
xacc = lambda t: -(ra * rw * rw) * np.sin(rw * t)
sFF = 1
uFF = lambda t: sFF * bg * (xacc(t))
stateDep = False
#==[3] Trajectory Tracker
theController = linear()
theController.set(K)
#ceom = simController.linearControlSys(A, B)
ceom = theController.systemDynamics(A, B)
solver = solver(ceom, dt)
#theController = controller.linear()
ctrl = theController.tracker(rsig, uFF, stateDep)
csim = simController(solver, ctrl)
#csim.initialize(tspan, istate)
sol = csim.simulate(tspan, istate)
#%==[4] Plot outcomes.
plt.figure(1)
plt.plot(sol.t, sol.x[0, :], 'b', sol.t, sol.x[1, :], 'g-')
# hold on;
# plot(sol.t, rsig(sol.t), ':');
# grid on;
# hold off;
plt.figure(2)
plt.plot(sol.t, sol.u[0, :], 'b', sol.t, uFF(sol.t), 'g-.')
#grid on;
def initialize(istate, desTraj):
cfS.controller.tracker(desTraj)
theSim = simController(solver, cfS.controller.compute)
theSim.initializeByStruct(desTraj.tspan, istate)
return theSim
import sys
sys.path.append("..")
from simController import simController
import numpy as np
import pybullet as p
physicsClientId = p.connect(p.GUI)
sim = simController(timeStep=1. / 500.,
stepsPerControlLoop=50,
physicsClientId=physicsClientId)
sim.terrain.generate(cellHeightScale=0.0, perlinHeightScale=0.0)
sim.resetClifford()
for i in range(20):
throttleAction = 10
steerAction = 0.5
data = sim.controlLoopStep([throttleAction, steerAction])
#print(data)
p.disconnect(physicsClientId=physicsClientId)
"susDamping": 100,
"susSpring": 50000,
"traction": 1.25,
"masScale": 1.0
}
#terrainParams = {"AverageAreaPerCell":1,
# "cellPerlinScale":5,
# "cellHeightScale":0.35,
# "smoothing":0.7,
# "perlinScale":2.5,
# "perlinHeightScale":0.1}
cliffordParam1 = cliffordEasyNovice
clifford1 = simController(timeStep=1. / 500.,
stepsPerControlLoop=50,
numSolverIterations=300,
physicsClientId=physicsClientId,
cliffordParams=cliffordParam1,
terrainParams=terrainParams,
moveThreshold=moveThreshold,
maxStopMoveLength=maxStopMoveLength)
sameSimComp = False
if sameSimComp:
clifford2 = simController(timeStep=1. / 500.,
stepsPerControlLoop=50,
numSolverIterations=300,
physicsClientId=physicsClientId,
cliffordParams=cliffordParam2,
existingTerrain=clifford1.terrain)
ignoreCollisions(clifford1.clifford.cliffordID,
clifford2.clifford.cliffordID)
clifford1.resetClifford(doFall=False)
clifford2.resetClifford()
length = 1
else:
length = len(t)
return np.vstack((np.sin(w0*t), (1-np.cos(w0*t)), zAmplitude*np.sin(zw0*t),\
w0*np.cos(w0*t), w0*np.sin(w0*t), zAmplitude*zw0*np.cos(zw0*t),\
-(w0**2)*np.sin(w0*t), (w0**2)*np.cos(w0*t), -zAmplitude*(zw0**2)*np.sin(zw0*t)))
fCurve = carousel
path = Curves.Explicit(fCurve, tSpan)
desTraj = trajectory.Path(path, tSpan)
theSolver = SimFirstOrder(theGroup, theCEOM, dt)
tPtsController = timepoints(pathgen, theController, ts)
theController.tracker(desTraj)
tPtsController.tracker(desTraj)
theSim = simController(theSolver, tPtsController)
startT = vec2Tangent(desTraj.x(0))
theSim.initialize(tSpan, startT.base())
sol = theSim.simulate()
plt.figure(1)
ax = plt.axes(projection='3d')
t = np.linspace(tSpan[0], tSpan[1], 10)
tplt = np.linspace(tSpan[0], tSpan[1], 100)
xDes = desTraj.x(tplt)
ax.plot(xDes[0,:], xDes[1,:], xDes[2,:])
plt.title("Desired frames")
for i in t:
mapToSE3(desTraj.x(i)).plot(ax=ax, scale=0.5)
import pdb
'''
Tests stabilizer ability of the linearSE3 class
'''
theGroup = Lie.group.SE3.Homog
theCEOM = linearSE3.systemDynamics()
dt = 0.10
theSolver = SimFirstOrder(theGroup, theCEOM, dt)
theController = linearSE3(xeq=theGroup(), ueq=np.zeros((6,1)))
theController.set(np.eye(6))
theSim = simController(theSolver, theController.stabilizer())
tSpan = np.array([0, 5])
initFlag = 2
if(initFlag == 0):
g0 = theGroup(x=np.array([1, 1, 1]).reshape((3,1)), R=np.eye(3))
elif(initFlag == 1):
e1 = np.array([2, 5, -6]).reshape((3,1))
e1 = e1/np.linalg.norm(e1)
e2 = np.array([0, 6, 5]).reshape((3,1))
e2 = e2/np.linalg.norm(e2)
e3 = np.cross(np.squeeze(e1), np.squeeze(e2))
g0 = theGroup(x=np.zeros((3,1)), R=np.hstack((e1, e2, e3[:, np.newaxis])))
elif(initFlag == 2):
R = np.matmul(np.matmul(theGroup.RotX(np.pi / 2), theGroup.RotY(np.pi / 3)),
theGroup.RotZ(np.pi / 4))
x = np.array([[5], [3], [2]])
gf = theGroup(R=R, x=x)
optS1 = FlightOptParams(init=5, final=3)
fp1 = Flight3D(gi, gf, tspan=tSpan, bezierOrder=order, optParams=optS1)
fp1.spec.vec2state = lambda x: x
fp1.optimizeBezierPath()
desTraj = fp1
#--[2.2] Simulate
#
theSim = simController(theSolver, theController.tracker(desTraj))
theSim.initialize(tSpan, gi)
sol = theSim.simulate()
# Plot
plt.figure(1)
ax = plt.axes(projection='3d')
t = np.linspace(tSpan[0], tSpan[1], 10)
tplt = np.linspace(tSpan[0], tSpan[1], 100)
xDes = desTraj.x(tplt)
ax.plot(xDes[0, :], xDes[1, :], xDes[2, :])
plt.title("Desired frames")
for i in t:
mapToSE3(desTraj.x(i)).plot(ax=ax, scale=0.5)
ax.set_box_aspect(
[ub - lb for lb, ub in (getattr(ax, f'get_{a}lim')() for a in 'xyz')])
def theInitializer(tspan, istate, rsig, uFF, statedep=False):
control = cfs.controller.tracker(rsig, uFF, statedep)
theSim = simController(solver, control)
theSim.initializeByStruct(tspan, istate)
return theSim