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)
usedControlInputs=newCoordIndices,
defaultJointOrientation=np.eye(3))
newCoordIndices = [2]
joint2to3 = JointPivotX(name="2To3",
childLink=link3,
parentLink=link2,
parentFollowerNumber=0,
usedGeneralizedCoordinates=newCoordIndices,
usedControlInputs=newCoordIndices,
defaultJointOrientation=np.eye(3))
initialPosition = np.array([np.pi / 4, -2 * np.pi / 3, 1 * np.pi / 5])
linkArray = [groundLink, link1, link2, link3]
chain = Chain(linkArray)
chain.name = "Three link chain"
blank_chain = deepcopy(chain)
blank_chain.update(initialPosition)
print(chain)
engine = SymbolicEngine(chain.linkArray)
deriveJacobiansForlinkArray(engine)
H = deriveJSIM(engine)
iN, dH = deriveGeneralizedInertialForces_dH(engine, H)
g = deriveGeneralizedGravitationalForces(engine)
def getThreeLinkChain():
groundLink = GroundLink()
link1 = Link(name="Link1",
order=1,
inertia=np.diag([
1 / 12 * 10 * 0.5, 1 / 12 * 10 * 0.5,
0.1 * 1 / 12 * 10 * 0.5
]),
mass=10,
relativeBase=[0, 0, 0],
relativeFollower=[[0, 0, 0.5]],
relativeCoM=[0, 0, 0.25])
link2 = Link(name="Link2",
order=1,
inertia=np.diag([
1 / 12 * 10 * 0.5, 1 / 12 * 10 * 0.5,
0.1 * 1 / 12 * 10 * 0.5
]),
mass=10,
relativeBase=[0, 0, 0],
relativeFollower=[[0, 0, 0.5]],
relativeCoM=[0, 0, 0.25])
link3 = Link(name="Link3",
order=1,
inertia=np.diag([
1 / 12 * 10 * 0.5, 1 / 12 * 10 * 0.5,
0.1 * 1 / 12 * 10 * 0.5
]),
mass=10,
relativeBase=[0, 0, 0],
relativeFollower=[[0, 0, 0.5]],
relativeCoM=[0, 0, 0.25])
genCoordIndex = 0
newCoordIndices = [0]
jointGto1 = JointPivotX(name="GroundToFirst",
childLink=link1,
parentLink=groundLink,
parentFollowerNumber=0,
usedGeneralizedCoordinates=newCoordIndices,
usedControlInputs=newCoordIndices,
defaultJointOrientation=np.eye(3))
newCoordIndices = [1]
joint1to2 = JointPivotX(name="1To2",
childLink=link2,
parentLink=link1,
parentFollowerNumber=0,
usedGeneralizedCoordinates=newCoordIndices,
usedControlInputs=newCoordIndices,
defaultJointOrientation=np.eye(3))
newCoordIndices = [2]
joint2to3 = JointPivotX(name="2To3",
childLink=link3,
parentLink=link2,
parentFollowerNumber=0,
usedGeneralizedCoordinates=newCoordIndices,
usedControlInputs=newCoordIndices,
defaultJointOrientation=np.eye(3))
linkArray = [groundLink, link1, link2, link3]
return Chain(linkArray)
def getCheetahChain():
cheetahLinks = getLinkArrayFromURDF(
os.path.abspath("./SrdPy/examples/cheetah/cheetah/urdf/cheetah.urdf"),
True)
cheetahChain = Chain(cheetahLinks)
return cheetahChain
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.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)
T = deriveControlMap(engine)
n = A_table.shape[2]
m = B_table.shape[2]
K_table = np.zeros((count, m, n))
for i in range(count):
K, S, CLP = lqr(A_table[i], B_table[i], Q_table[i], R_table[i])
K_table[i] = K
return K_table
iiwaLinks = getLinkArrayFromURDF(
os.path.abspath("./examples/iiwa/iiwa14.urdf"), True)
iiwaChain = Chain(iiwaLinks)
import pickle
print(iiwaChain)
initialPosition = np.zeros(7)
blank_chain = deepcopy(iiwaChain)
blank_chain.update(initialPosition)
engine = SymbolicEngine(iiwaChain.linkArray)
deriveJacobiansForlinkArray(engine)
H = deriveJSIM(engine)
C = deriveCmatrixViaChristoffel(engine, H)
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)
H = deriveJSIM(engine)
np.set_printoptions(linewidth=1000)
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.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/urdf/a1.urdf"), True)
a1Chain = Chain(a1Links)
print(a1Chain)
initialPosition = np.zeros(18)
engine = SymbolicEngine(a1Chain.linkArray)
deriveJacobiansForlinkArray(engine)
H = deriveJSIM(engine)
iN, dH = deriveGeneralizedInertialForces_dH(engine, H)
g = deriveGeneralizedGravitationalForces(engine)
d = deriveGeneralizedDissipativeForcesUniform(engine, 1)
T = deriveControlMapFloating(engine)
description_gen_coord_model = generateDynamicsGeneralizedCoordinatesModel(
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])