def atlasImportExample():
dynamicsFolderName = "atlas"
print(os.getcwd())
atlasLinks = getLinkArrayFromURDF(
os.path.abspath("./SrdPy/examples/atlas/atlas_v5.urdf"), True)
atlasChain = Chain(atlasLinks)
atlasChain.update(np.array([0] * 36))
engine = SymbolicEngine(atlasChain.linkArray)
deriveJacobiansForlinkArray(engine)
H = deriveJSIM(engine)
iN, dH = deriveGeneralizedInertialForces_dH(engine, H)
g = deriveGeneralizedGravitationalForces(engine)
d = deriveGeneralizedDissipativeForcesUniform(engine, 1)
T = deriveControlMap(engine)
# NaiveControlMap
description_gen_coord_model = generateDynamicsGeneralizedCoordinatesModel(
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="./" + dynamicsFolderName + "/Dynamics")
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="./" + dynamicsFolderName + "/Linearization")
handlerGeneralizedCoordinatesModel = getGeneralizedCoordinatesModelHandlers(
description_gen_coord_model)
handlerLinearizedModel = getLinearizedModelHandlers(
description_linearization)
def getCheetahChain():
cheetahLinks = getLinkArrayFromURDF(
os.path.abspath("./SrdPy/examples/cheetah/cheetah/urdf/cheetah.urdf"),
True)
cheetahChain = Chain(cheetahLinks)
return cheetahChain
from SrdPy.Controllers import *
from SrdPy.Visuals import Visualizer
from SrdPy import SymbolicEngine
from SrdPy import plotGeneric
from copy import deepcopy
from casadi import *
from SrdPy.TableGenerators import *
from SrdPy import Chain
from SrdPy import Profiler
import numpy as np
from scipy.integrate import solve_ivp
import os
a1Links = getLinkArrayFromURDF(
os.path.abspath("./SrdPy/examples/a1/a1/urdf/a1.urdf"), True)
a1Chain = Chain(a1Links)
blank_chain = deepcopy(a1Chain)
print(a1Chain)
initialPosition = np.zeros(18)
engine = SymbolicEngine(a1Chain.linkArray)
deriveJacobiansForlinkArray(engine)
H = deriveJSIM(engine)
np.set_printoptions(linewidth=1000)
iN, dH = deriveGeneralizedInertialForces_dH(engine, H)
g = deriveGeneralizedGravitationalForces(engine)
d = deriveGeneralizedDissipativeForcesUniform(engine, 1)
T = deriveControlMapFloating(engine)
from SrdPy.Controllers import *
from SrdPy.Visuals import Visualizer
from SrdPy import SymbolicEngine
from SrdPy import plotGeneric
from copy import deepcopy
from casadi import *
from SrdPy.TableGenerators import *
from SrdPy import Chain
from SrdPy import Profiler
import numpy as np
from scipy.integrate import solve_ivp
import os
iiwaLinks = getLinkArrayFromURDF(
os.path.abspath("./SrdPy/examples/iiwa/iiwa14.urdf"), True)
iiwaChain = Chain(iiwaLinks)
print(iiwaChain)
initialPosition = np.zeros(7)
blank_chain = deepcopy(iiwaChain)
blank_chain.update(initialPosition)
engine = SymbolicEngine(iiwaChain.linkArray)
deriveJacobiansForlinkArray(engine)
H = deriveJSIM(engine)
iN, dH = deriveGeneralizedInertialForces_dH(engine, H)
g = deriveGeneralizedGravitationalForces(engine)
d = deriveGeneralizedDissipativeForcesUniform(engine, 1)
from SrdPy.Visuals import Visualizer
from SrdPy import SymbolicEngine
from SrdPy import plotGeneric
from copy import deepcopy
from casadi import *
from SrdPy.TableGenerators import *
from SrdPy import Chain
from SrdPy import Profiler
import numpy as np
from scipy.integrate import solve_ivp
import os
cheetahLinks = getLinkArrayFromURDF(
os.path.abspath(
os.path.dirname(sys.argv[0]) + "/cheetah/urdf/cheetah.urdf"), True)
cheetahChain = Chain(cheetahLinks)
remap = [
'trunk', 'FL_hip', 'FL_thigh', 'FL_calf', 'FL_foot', 'FR_hip', 'FR_thigh',
'FR_calf', 'FR_foot', 'RL_hip', 'RL_thigh', 'RL_calf', 'RL_foot', 'RR_hip',
'RR_thigh', 'RR_calf', 'RR_foot'
]
cheetahChain.remapGenCoords(remap)
print(cheetahChain)
initialPosition = np.zeros(18)
engine = SymbolicEngine(cheetahChain.linkArray)
deriveJacobiansForlinkArray(engine)
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()
from SrdPy.plotGeneric import plotGeneric
from SrdPy.URDFUtils import getLinkArrayFromURDF
from SrdPy.Visuals import Visualizer
from copy import deepcopy
from SrdPy.TableGenerators import *
from SrdPy import Chain
import numpy as np
import os
cheetahLinks = getLinkArrayFromURDF(
os.path.abspath("./SrdPy/examples/cheetah/cheetah/urdf/cheetah.urdf"),
True)
cheetahChain = Chain(cheetahLinks)
remap = [
'trunk', 'FL_hip', 'FL_thigh', 'FL_calf', 'FL_foot', 'FR_hip', 'FR_thigh',
'FR_calf', 'FR_foot', 'RL_hip', 'RL_thigh', 'RL_calf', 'RL_foot', 'RR_hip',
'RR_thigh', 'RR_calf', 'RR_foot'
]
cheetahChain.remapGenCoords(remap)
print(cheetahChain)
initialPosition = np.zeros(18)
blank_chain = deepcopy(cheetahChain)
blank_chain.update(initialPosition)
with open('anim_array.npy', 'rb') as f:
q = np.load(f)
blank_chain.update(q[0])
from SrdPy.Controllers import *
from SrdPy.Visuals import Visualizer
from SrdPy import SymbolicEngine
from SrdPy import plotGeneric
from copy import deepcopy
from casadi import *
from SrdPy.TableGenerators import *
from SrdPy import Chain
from SrdPy import Profiler
import numpy as np
from scipy.integrate import solve_ivp
import os
cheetahLinks = getLinkArrayFromURDF(
os.path.abspath("./cheetah/urdf/cheetah.urdf"), True)
cheetahChain = Chain(cheetahLinks)
remap = [
'trunk', 'FL_hip', 'FL_thigh', 'FL_calf', 'FL_foot', 'FR_hip', 'FR_thigh',
'FR_calf', 'FR_foot', 'RL_hip', 'RL_thigh', 'RL_calf', 'RL_foot', 'RR_hip',
'RR_thigh', 'RR_calf', 'RR_foot'
]
cheetahChain.remapGenCoords(remap)
print(cheetahChain)
initialPosition = np.zeros(18)
engine = SymbolicEngine(cheetahChain.linkArray)
deriveJacobiansForlinkArray(engine)
H = deriveJSIM(engine)
