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