class AnymalFreeFwdDynamicsTest(ActionModelAbstractTestCase):
ROBOT_MODEL = example_robot_data.loadANYmal().model
STATE = crocoddyl.StateMultibody(ROBOT_MODEL)
ACTUATION = crocoddyl.ActuationModelFloatingBase(STATE)
COST_SUM = crocoddyl.CostModelSum(STATE, ACTUATION.nu)
COST_SUM.addCost("xReg", crocoddyl.CostModelState(STATE, ACTUATION.nu), 1e-7)
COST_SUM.addCost("uReg", crocoddyl.CostModelControl(STATE, ACTUATION.nu), 1e-7)
MODEL = crocoddyl.DifferentialActionModelFreeFwdDynamics(STATE, ACTUATION, COST_SUM)
MODEL_DER = DifferentialFreeFwdDynamicsDerived(STATE, ACTUATION, COST_SUM)
from crocoddyl import (ContactModel6D, ContactModelMultiple, CostModelSum,
DifferentialActionModelNumDiff)
from crocoddyl import ActuationModelFloatingBase as ActuationModelFreeFloating
from crocoddyl import CostModelCentroidalMomentum as CostModelMomentum
from crocoddyl import DifferentialActionModelContactFwdDynamics as DifferentialActionModelFloatingInContact
from crocoddyl import StateMultibody as StatePinocchio
from crocoddyl import FramePlacement
from example_robot_data import loadANYmal
from crocoddyl.utils import a2m, m2a
from pinocchio.utils import rand
from test_utils import NUMDIFF_MODIFIER, assertNumDiff
# Loading Talos arm with FF TODO use a biped or quadruped
# -----------------------------------------------------------------------------
robot = loadANYmal()
robot.model.armature[6:] = 1.
qmin = robot.model.lowerPositionLimit
qmin[:7] = -1
robot.model.lowerPositionLimit = qmin
qmax = robot.model.upperPositionLimit
qmax[:7] = 1
robot.model.upperPositionLimit = qmax
rmodel = robot.model
rdata = rmodel.createData()
# -----------------------------------------
q = pinocchio.randomConfiguration(rmodel)
v = rand(rmodel.nv)
x = m2a(np.concatenate([q, v]))
u = m2a(rand(rmodel.nv - 6))
import sys import numpy as np import crocoddyl import example_robot_data import pinocchio from crocoddyl.utils.quadruped import SimpleQuadrupedalGaitProblem, plotSolution WITHDISPLAY = 'display' in sys.argv or 'CROCODDYL_DISPLAY' in os.environ WITHPLOT = 'plot' in sys.argv or 'CROCODDYL_PLOT' in os.environ crocoddyl.switchToNumpyMatrix() # Loading the anymal model anymal = example_robot_data.loadANYmal() lims = anymal.model.effortLimit lims *= 0.4 # reduced artificially the torque limits anymal.model.effortLimit = lims lims = anymal.model.velocityLimit lims *= 0.5 anymal.model.velocityLimit = lims # Defining the initial state of the robot q0 = anymal.model.referenceConfigurations['standing'].copy() v0 = pinocchio.utils.zero(anymal.model.nv) x0 = np.concatenate([q0, v0]) # Setting up the 3d walking problem lfFoot, rfFoot, lhFoot, rhFoot = 'LF_FOOT', 'RF_FOOT', 'LH_FOOT', 'RH_FOOT' gait = SimpleQuadrupedalGaitProblem(anymal.model, lfFoot, rfFoot, lhFoot,
class ANYmalKinovaTest(RobotTestCase):
RobotTestCase.ROBOT = example_robot_data.loadANYmal(withArm="kinova")
RobotTestCase.NQ = 27
RobotTestCase.NV = 24
class ANYmalTest(RobotTestCase):
RobotTestCase.ROBOT = example_robot_data.loadANYmal()
RobotTestCase.NQ = 19
RobotTestCase.NV = 18
