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)
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)
with open('anim_array.npy', 'wb') as f:
np.save(f, solution_tape[:, :n])
chainLinks = getLinkArrayFromURDF(
os.path.abspath("./examples/iiwa/iiwa14.urdf"), True)
chain = Chain(chainLinks)
print(chain)
blank_chain = deepcopy(chain)
blank_chain.update(initialPosition)
with open('anim_array.npy', 'rb') as f:
q = np.load(f)
blank_chain.update(q[0])
plotGeneric(np.arange(q.shape[0]), q, plot=True)
vis = Visualizer()
vis.animate(blank_chain, q, framerate=0.1, showMeshes=True)
print("Done")
engine,
H=H,
c=(iN + g + d),
T=T,
functionName_H="g_dynamics_H",
functionName_c="g_dynamics_c",
functionName_T="g_dynamics_T",
casadi_cCodeFilename="g_dynamics_generalized_coordinates",
path="./a1/Dynamics")
handlerGeneralizedCoordinatesModel = GeneralizedCoordinatesModelHandler(
description_gen_coord_model)
description_linearization = generateDynamicsLinearization(
engine,
H=H,
c=(iN + g + d),
T=T,
functionName_A="g_linearization_A",
functionName_B="g_linearization_B",
functionName_c="g_linearization_c",
casadi_cCodeFilename="g_dynamics_linearization",
path="./a1/Linearization")
handlerLinearizedModel = LinearizedModelHandler(description_linearization)
vis = Visualizer()
initialPosition = np.zeros(18)
blank_chain.update(initialPosition)
vis.show(blank_chain, showMeshes=True)
input()
from SrdPy.URDFUtils import getLinkArrayFromURDF
from SrdPy import Chain
import numpy as np
from SrdPy.Visuals import Visualizer
import os
print(os.getcwd())
iiwaLinks = getLinkArrayFromURDF(os.path.abspath("./iiwa14.urdf"),True)
iiwaChain = Chain(iiwaLinks)
iiwaChain.update(np.array([1]*7))
vis = Visualizer()
vis.show(iiwaChain,True)
input()