Python getLinkArrayFromURDF Examples

Example #1

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)

Example #2

File: getCheetahChain.py Project: RumblingTurtle/SrdPy

def getCheetahChain():
    cheetahLinks = getLinkArrayFromURDF(
        os.path.abspath("./SrdPy/examples/cheetah/cheetah/urdf/cheetah.urdf"),
        True)
    cheetahChain = Chain(cheetahLinks)
    return cheetahChain

Example #3

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)

Example #4

File: pyBulletTest.py Project: RumblingTurtle/SrdPy

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)

Example #5

File: tableBasedSimulation.py Project: RumblingTurtle/SrdPy

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)

Example #6

File: iiwaUrdfImportExample.py Project: RumblingTurtle/SrdPy

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()

Example #7

File: animationTest.py Project: RumblingTurtle/SrdPy

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])

Example #8

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)