# nmhe.addGaussNewtonObjF(nmhe('x',timestep=k) - xTraj[k][0])
# nmhe.addGaussNewtonObjF(nmhe('z',timestep=k) - xTraj[k][1])
# nmhe.addGaussNewtonObjF(nmhe('dx',timestep=k) - xTraj[k][2])
# nmhe.addGaussNewtonObjF(nmhe('dz',timestep=k) - xTraj[k][3])
# obj += (nmhe('x',timestep=k) - xTraj[k][0])**2
# obj += (nmhe('z',timestep=k) - xTraj[k][1])**2
# obj += (nmhe('dx',timestep=k) - xTraj[k][2])**2
# obj += (nmhe('dz',timestep=k) - xTraj[k][3])**2
# obj += 1e-8*nmhe('x',timestep=k)**2
# obj += 1e-8*nmhe('z',timestep=k)**2
# obj += 1e-8*nmhe('dx',timestep=k)**2
# obj += 1e-8*nmhe('dz',timestep=k)**2
# obj += 1e-8*nmhe('m')**2
mhe.setObj(obj)
# nmhe.constrain(nmhe('dz',timestep=0)**2,'<=',1000)
# get u
uTraj = C.DMatrix([[traj.lookup(name, timestep=k) for k in range(nk)]
for name in dae.uNames()]).T
endTime = traj.tgrid[1, 0, 0] - traj.tgrid[0, 0, 0]
mhe.makeSolver(endTime, traj=traj)
mhe.runSolver(uTraj, traj)
# # callback function
# class MyCallback:
# def __init__(self):
# self.iter = 0
# def __call__(self,f,*args):
# obj += (nmhe('x',timestep=k) - xTraj[k][0])**2
# obj += (nmhe('z',timestep=k) - xTraj[k][1])**2
# obj += (nmhe('dx',timestep=k) - xTraj[k][2])**2
# obj += (nmhe('dz',timestep=k) - xTraj[k][3])**2
# obj += 1e-8*nmhe('x',timestep=k)**2
# obj += 1e-8*nmhe('z',timestep=k)**2
# obj += 1e-8*nmhe('dx',timestep=k)**2
# obj += 1e-8*nmhe('dz',timestep=k)**2
# obj += 1e-8*nmhe('m')**2
# nmhe.constrain(nmhe('dz',timestep=0)**2,'<=',1000)
nmhe.setObj(obj)
uTraj = C.DMatrix(np.concatenate(uTraj))
nmhe.makeSolver(dt)
nmhe.runSolver(uTraj)
## newton.isolver.input(0).set(x0)
# newton.isolver.setInput(x0,0)
# newton.isolver.setInput(0,1) # torque
# newton.isolver.setInput(0.3,2) # mass
# newton.isolver.evaluate()
# newton.isolver.evaluate()
#
# j = newton.isolver.jacobian(0,1)
# j.init()
# nmhe.addGaussNewtonObjF(nmhe('x',timestep=k) - xTraj[k][0])
# nmhe.addGaussNewtonObjF(nmhe('z',timestep=k) - xTraj[k][1])
# nmhe.addGaussNewtonObjF(nmhe('dx',timestep=k) - xTraj[k][2])
# nmhe.addGaussNewtonObjF(nmhe('dz',timestep=k) - xTraj[k][3])
# obj += (nmhe('x',timestep=k) - xTraj[k][0])**2
# obj += (nmhe('z',timestep=k) - xTraj[k][1])**2
# obj += (nmhe('dx',timestep=k) - xTraj[k][2])**2
# obj += (nmhe('dz',timestep=k) - xTraj[k][3])**2
# obj += 1e-8*nmhe('x',timestep=k)**2
# obj += 1e-8*nmhe('z',timestep=k)**2
# obj += 1e-8*nmhe('dx',timestep=k)**2
# obj += 1e-8*nmhe('dz',timestep=k)**2
# obj += 1e-8*nmhe('m')**2
mhe.setObj(obj)
# nmhe.constrain(nmhe('dz',timestep=0)**2,'<=',1000)
# get u
uTraj = C.DMatrix([[traj.lookup(name,timestep=k) for k in range(nk)] for name in dae.uNames()]).T
endTime = traj.tgrid[1,0,0] - traj.tgrid[0,0,0]
mhe.makeSolver(endTime,traj=traj)
mhe.runSolver(uTraj,traj)
# # callback function
# class MyCallback:
# def __init__(self):
# self.iter = 0
# def __call__(self,f,*args):
nmhe.addGaussNewtonObjF(nmhe('dz', timestep=k) - xTraj[k][3])
# obj += (nmhe('x',timestep=k) - xTraj[k][0])**2
# obj += (nmhe('z',timestep=k) - xTraj[k][1])**2
# obj += (nmhe('dx',timestep=k) - xTraj[k][2])**2
# obj += (nmhe('dz',timestep=k) - xTraj[k][3])**2
# obj += 1e-8*nmhe('x',timestep=k)**2
# obj += 1e-8*nmhe('z',timestep=k)**2
# obj += 1e-8*nmhe('dx',timestep=k)**2
# obj += 1e-8*nmhe('dz',timestep=k)**2
# obj += 1e-8*nmhe('m')**2
# nmhe.constrain(nmhe('dz',timestep=0)**2,'<=',1000)
nmhe.setObj(obj)
uTraj = C.DMatrix(np.concatenate(uTraj))
nmhe.makeSolver(dt)
nmhe.runSolver(uTraj)
## newton.isolver.input(0).set(x0)
# newton.isolver.setInput(x0,0)
# newton.isolver.setInput(0,1) # torque
# newton.isolver.setInput(0.3,2) # mass
# newton.isolver.evaluate()
# newton.isolver.evaluate()
#
# j = newton.isolver.jacobian(0,1)
# j.init()
# j.setInput(x0,0)
# j.setInput(0,1)
