def __init__(self):
cheetahChain = getCheetahChain()
initialPosition = np.zeros(18)
blank_chain = deepcopy(cheetahChain)
blank_chain.update(initialPosition)
engine = SymbolicEngine(cheetahChain.linkArray)
deriveJacobiansForlinkArray(engine)
H = deriveJSIM(engine)
iN, dH = deriveGeneralizedInertialForces_dH(engine, H)
g = deriveGeneralizedGravitationalForces(engine)
d = deriveGeneralizedDissipativeForcesUniform(engine, 1)
T = deriveControlMapFloating(engine)
constraint3 = np.squeeze(engine.links["RL_calf"].absoluteFollower)
CoM = engine.getCoM()
task = np.hstack([constraint3, CoM])
description_IK = generateSecondDerivativeJacobians(
engine,
task=task,
functionName_Task="g_InverseKinematics_Task",
functionName_TaskJacobian="g_InverseKinematics_TaskJacobian",
functionName_TaskJacobianDerivative=
"g_InverseKinematics_TaskJacobian_derivative",
casadi_cCodeFilename="g_InverseKinematics",
path="./cheetah/InverseKinematics")
self.ikModelHandler = IKModelHandler(description_IK, engine.dof,
task.shape[0])
def __init__(self):
self.initialPositions = [[1, 1, 1], [np.pi, 0, np.pi],
[np.pi, np.pi, np.pi]]
self.chain = getThreeLinkChain()
self.engine = SymbolicEngine(self.chain.linkArray)
self.generateJSIM()
self.deriveLinearizationVariables()
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))
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)
d = deriveGeneralizedDissipativeForcesUniform(engine, 1)
T = deriveControlMap(engine)
# NaiveControlMap
description_gen_coord_model = generateDynamicsGeneralizedCoordinatesModel(
engine,
H=H,
c=(iN + g + d),
T=T,
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)
description_gen_coord_model = generateDynamicsGeneralizedCoordinatesModel(
engine,
H=H,
c=(iN + g + d),
T=T,
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)
description_gen_coord_model = generateDynamicsGeneralizedCoordinatesModel(
engine,
H=H,
c=(iN + g + d),
T=T,
functionName_H="g_dynamics_H",
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)
iN, dH = deriveGeneralizedInertialForces_dH(engine, H)
g = deriveGeneralizedGravitationalForces(engine)
d = deriveGeneralizedDissipativeForcesUniform(engine, 1)
T = deriveControlMapFloating(engine)
description_gen_coord_model = generateDynamicsGeneralizedCoordinatesModel(
engine,
H=H,
c=(iN + g + d),
T=T,