import matplotlib.pylab as plt from specpath import acadoPath,dataRootPath,acadoTxtPath RANDOM_SEED = 999 # int((time.time()%10)*1000) print "Seed = %d" % RANDOM_SEED np .random.seed (RANDOM_SEED) random.seed (RANDOM_SEED) #env = Pendulum(2,withDisplay=True) # Continuous pendulum env = Pendulum(2,length=.5,mass=3.0,armature=.2,withDisplay=False) env.withSinCos = False # State is dim-3: (cosq,sinq,qdot) ... NX = env.nobs # ... training converges with q,qdot with 2x more neurones. NU = env.nu # Control is dim-1: joint torque env.vmax = 100. env.Kf = np.diagflat([ 0.2, 2. ]) env.modulo = False env.DT = 0.15 env.NDT = 1 #env.umax = 15. #env.umax = (15.,15.) env.umax = np.matrix([5.,10.]).T NSTEPS = 32 env.qlow[1] = -np.pi env.qup [1] = np.pi # Shortcut function to convert SE3 to 7-dof vector. M2gv = lambda M: XYZQUATToViewerConfiguration(se3ToXYZQUAT(M))
dataflat = np.load('data/planner/double/5_15/grid_refine.npy')
data = []
for i, d in enumerate(dataflat):
data.append(Data(*d))
### --- Environment
env = Pendulum(2, length=.5, mass=3.0, armature=10.)
env.withSinCos = True # State is dim-3: (cosq,sinq,qdot) ...
NX = env.nobs # ... training converges with q,qdot with 2x more neurones.
NU = env.nu # Control is dim-1: joint torque
env.DT = 0.15
env.NDT = 15
env.Kf = 2.0 # 1.0
env.vmax = 100
env.umax = 15.
env.modulo = True
NSTEPS = 50
BATCH_SIZE = 128
RESTORE = '' #'netvalues/double/actorcritic_double.59999'
RENDERACTION = [
'rollout',
] # 'draw', 'rollout'
RENDERRATE = 2000 # Render rate (rollout and plot) during training (0 = no)
### --- Q-value and policy networks
class QValueNetwork:
### --- UNIT TEST ---
### --- UNIT TEST ---
### --- UNIT TEST ---
if __name__ == '__main__':
from pendulum import Pendulum
env = Pendulum(1,withDisplay=False) # Continuous pendulum
env.withSinCos = False # State is dim-3: (cosq,sinq,qdot) ...
NX = env.nobs # ... training converges with q,qdot with 2x more neurones.
NU = env.nu # Control is dim-1: joint torque
env.vmax = 100.
env.Kf = 0.2
env.modulo = False
env.DT = 0.05
env.NDT = 1
T = .5
NSTEPS = int(round(T/env.DT))
x1 = np.matrix([ .2, .1]).T
x2 = np.matrix([ .2, -.1]).T
acado = AcadoConnect(model=env.model)
acado.setTimeInterval(T)
acado.options['steps'] = NSTEPS
acado.options['shift'] = 0
acado.options['iter'] = 100
import acado_runner
plt.ion()
env = Pendulum(2, length=.5, mass=3.0, armature=10.)
acado = acado_runner.AcadoRunner(
"/home/nmansard/src/pinocchio/pycado/build/unittest/discrete_double_pendulum"
)
env.umax = 15.
env.vmax = 100
env.modulo = False
NSTEPS = 50
env.DT = 0.2 #25e-3
env.NDT = 1
env.Kf = 1.0
x0 = np.matrix([0.3, 0.3, 0., 0.]).T
def controlPD(withPlot=True):
'''
Compute a control trajectory by rollout a PD controller.
'''
Kp = 50.
Kv = 2 * np.sqrt(Kp) * .5
ratioK = 1.0
#xdes = np.matrix([3.14,0,0,0]).T
xdes = np.matrix([0., 0, 0, 0]).T
K = np.matrix(
np.hstack([
np.diagflat([ratioK * Kp, Kp]),
