def plotIKTable(IKModelHandler, timeTable, IKTable, tol=10**(-5)):
count = len(timeTable)
condition_number_tape = np.zeros((count, 1))
rank_tape = np.zeros((count, 1))
for i in range(count):
q = IKTable[i]
J = IKModelHandler.getJacobian(q)
condition_number_tape[i] = np.linalg.cond(J)
rank_tape[i] = np.linalg.matrix_rank(J, tol)
ax = plotGeneric(timeTable, IKTable, figureTitle='IK Solution', ylabel="q")
fig, ax = plt.subplots(2)
ax[0].plot(timeTable, condition_number_tape, 'r', label=r'$cond_1$')
ax[0].legend(loc='upper right')
ax[0].set_xlabel(r'$t, s$', fontsize=15)
ax[0].set_ylabel(r'$cond_i$', fontsize=15)
ax[0].set_title(r'condition number J', fontsize=18)
ax[0].grid(color='k', linestyle='-', linewidth=0.15)
ax[1].plot(timeTable, rank_tape, 'r', label=r'$cond_1$')
ax[1].legend(loc='upper right')
ax[1].set_xlabel(r'$t, s$', fontsize=15)
ax[1].set_ylabel(r'$rank_i$', fontsize=15)
ax[1].set_title(r'rank J', fontsize=18)
ax[1].grid(color='k', linestyle='-', linewidth=0.15)
plt.show(block=True)
handlerConstraints)
stateHandlerLogger = StateLoggerHandler(stateHandler, simulationHandler)
tickLogger = ProgressDisplayHandler(simulationHandler)
preprocessingHandlers = [
desiredStateHandler, stateSpaceHandler, desiredStateSpaceHandler,
gcModelEvaluator
]
controllerHandlers = [inverseDynamicsHandler, linearModelEvaluator, LQRHandler]
solverHandlers = [taylorSolverHandler]
loggerHandlers = [stateHandlerLogger, tickLogger]
simulationHandler.preprocessingHandlersArray = preprocessingHandlers
simulationHandler.controllerArray = controllerHandlers
simulationHandler.solverArray = solverHandlers
simulationHandler.loggerArray = loggerHandlers
simulationHandler.simulate()
plotGeneric(simulationHandler.timeLog[:-1],
stateHandlerLogger.q,
figureTitle="Q",
ylabel="q")
plotGeneric(simulationHandler.timeLog[:-1],
stateHandlerLogger.v,
figureTitle="V",
ylabel="v")
vis = Visualizer()
vis.animate(blank_chain, stateHandlerLogger.q, framerate=0.1)
K_table = my_generateLQRTable(A_table, B_table, np.tile(Q, [count, 1, 1]),
np.tile(R, [count, 1, 1]))
AA_table, cc_table = generateCloseLoopTable(A_table, B_table, c_table, K_table,
x_table, u_table)
ode_fnc_handle = ClosedLoopLinearSystemOdeFunctionHandler(
AA_table, cc_table, timeTable)
x0 = np.hstack((initialPosition, np.zeros(initialPosition.shape[0])))
sol = solve_ivp(ode_fnc_handle, [0, tf], x0, t_eval=timeTable, method="RK45")
time_table_0 = sol.t
solution_tape = sol.y.T
ax = plotGeneric(time_table_0, solution_tape, figureTitle="", ylabel="ODE")
ax = plotGeneric(timeTable,
x_table,
ylabel="linearmodel",
old_ax=ax,
plot=True)
ax = plotGeneric(timeTable,
solution_tape[:, :n],
figureTitle="position",
ylabel="q",
plot=True)
ax = plotGeneric(timeTable,
solution_tape[:, n:2 * n],
figureTitle="velocity",
ylabel="v",
AA_table, cc_table = generateCloseLoopTable(A_table, B_table, c_table, K_table,
x_table, u_table)
ode_fnc_handle = ClosedLoopConstrainedLinearSystemOdeFunctionHandler(
AA_table, cc_table, G_table, F_table, timeTable)
x0 = np.hstack((initialPosition, np.zeros(initialPosition.shape[0])))
sol = solve_ivp(ode_fnc_handle, [0, tf], x0, t_eval=timeTable, method="LSODA")
time_table_0 = sol.t
solution_tape = sol.y.T
ax = plotGeneric(time_table_0,
solution_tape,
figureTitle="",
ylabel="ODE",
plot=True)
ax = plotGeneric(timeTable, x_table, ylabel="linearmodel", plot=True)
ax = plotGeneric(timeTable,
solution_tape[:, :n],
figureTitle="position",
ylabel="q",
plot=True)
ax = plotGeneric(timeTable,
solution_tape[:, n:2 * n],
figureTitle="velocity",
ylabel="v",
plot=True)
A_table, B_table, c_table, x_table, u_table, dx_table = generateLinearModelTable(
handlerGeneralizedCoordinatesModel, handlerLinearizedModel,
ikSolutionHandler, timeTable)
N_table, G_table = generateConstraiedModelTable(handlerConstraints, x_table,
[])
Q = 100 * np.eye(2 * n)
R = 0.01 * np.eye(handlerGeneralizedCoordinatesModel.dofControl)
count = A_table.shape[0]
K_table = generateLQRTable(A_table, B_table, np.tile(Q, [count, 1, 1]),
np.tile(R, [count, 1, 1]))
AA_table, cc_table = generateCloseLoopTable(A_table, B_table, c_table, K_table,
x_table, u_table)
ode_fnc_handle = ClosedLoopLinearSystemOdeFunctionHandler(
AA_table, cc_table, timeTable)
x0 = np.hstack((initialPosition, np.zeros(initialPosition.shape[0])))
sol = solve_ivp(ode_fnc_handle, [0, tf], x0)
time_table_0 = sol.t
solution_tape = sol.y.T
ax = plotGeneric(time_table_0, solution_tape, figureTitle="", ylabel="ODE")
ax = plotGeneric(timeTable,
x_table,
ylabel="linearmodel",
old_ax=ax,
plot=True)
tickLogger = ProgressDisplayHandler(timeHandler)
preprocessingHandlers = [
desiredStateHandler, stateSpaceHandler, desiredStateSpaceHandler,
gcModelEvaluator
]
controllerHandlers = [inverseDynamicsHandler, linearModelEvaluator, LQRHandler]
solverHandlers = [taylorSolverHandler]
loggerHandlers = [stateHandlerLogger, tickLogger]
handlerUpdater = HandlerUpdater()
handlerUpdater.handlers = preprocessingHandlers + controllerHandlers + solverHandlers + loggerHandlers + [
timeHandler
]
for i in range(timeHandler.timeLog.shape[0]):
handlerUpdater.update()
plotGeneric(timeHandler.timeLog,
stateHandlerLogger.q,
figureTitle="Q",
ylabel="q")
plotGeneric(timeHandler.timeLog,
stateHandlerLogger.v,
figureTitle="V",
ylabel="v")
vis = Visualizer()
vis.animate(blank_chain, stateHandlerLogger.q, framerate=0.1)
vis.animate(blank_chain, stateHandlerLogger.q, framerate=0.1)
input()