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)
class AnymalIntegratedRK4Test(ActionModelAbstractTestCase): ROBOT_MODEL = example_robot_data.load('anymal').model STATE = crocoddyl.StateMultibody(ROBOT_MODEL) ACTUATION = crocoddyl.ActuationModelFloatingBase(STATE) COST_SUM = crocoddyl.CostModelSum(STATE, ACTUATION.nu) COST_SUM.addCost("xReg", crocoddyl.CostModelResidual(STATE, crocoddyl.ResidualModelState(STATE, ACTUATION.nu)), 1e-7) COST_SUM.addCost("uReg", crocoddyl.CostModelResidual(STATE, crocoddyl.ResidualModelControl(STATE, ACTUATION.nu)), 1e-7) DIFFERENTIAL = crocoddyl.DifferentialActionModelFreeFwdDynamics(STATE, ACTUATION, COST_SUM) MODEL = crocoddyl.IntegratedActionModelRK4(DIFFERENTIAL, 1e-3) MODEL_DER = IntegratedActionModelRK4Derived(DIFFERENTIAL, 1e-3)
def __init__(self, rmodel, rightFoot, leftFoot): self.rmodel = rmodel self.rdata = rmodel.createData() self.state = crocoddyl.StateMultibody(self.rmodel) self.actuation = crocoddyl.ActuationModelFloatingBase(self.state) # Getting the frame id for all the legs self.rfId = self.rmodel.getFrameId(rightFoot) self.lfId = self.rmodel.getFrameId(leftFoot) # Defining default state q0 = self.rmodel.referenceConfigurations["half_sitting"] self.rmodel.defaultState = np.concatenate([q0, np.zeros((self.rmodel.nv, 1))]) self.firstStep = True
def __init__(self, rmodel, lfFoot, rfFoot, lhFoot, rhFoot): self.rmodel = rmodel self.rdata = rmodel.createData() self.state = crocoddyl.StateMultibody(self.rmodel) self.actuation = crocoddyl.ActuationModelFloatingBase(self.state) # Getting the frame id for all the legs self.lfFootId = self.rmodel.getFrameId(lfFoot) self.rfFootId = self.rmodel.getFrameId(rfFoot) self.lhFootId = self.rmodel.getFrameId(lhFoot) self.rhFootId = self.rmodel.getFrameId(rhFoot) # Defining default state q0 = self.rmodel.referenceConfigurations["standing"] self.rmodel.defaultState = np.concatenate([q0, np.zeros(self.rmodel.nv)]) self.firstStep = True # Defining the friction coefficient and normal self.mu = 0.7 self.Rsurf = np.eye(3)
def __init__(self, rmodel, rightFoot, leftFoot, integrator='euler', control='zero'): """ Construct biped-gait problem. :param rmodel: robot model :param rightFoot: name of the right foot :param leftFoot: name of the left foot :param integrator: type of the integrator (options are: 'euler', and 'rk4') :param control: type of control parametrization (options are: 'zero', 'one', and 'rk4') """ self.rmodel = rmodel self.rdata = rmodel.createData() self.state = crocoddyl.StateMultibody(self.rmodel) self.actuation = crocoddyl.ActuationModelFloatingBase(self.state) # Getting the frame id for all the legs self.rfId = self.rmodel.getFrameId(rightFoot) self.lfId = self.rmodel.getFrameId(leftFoot) self.integrator = integrator if control == 'one': self.control = crocoddyl.ControlParametrizationModelPolyOne( self.actuation.nu) elif control == 'rk4': self.control = crocoddyl.ControlParametrizationModelPolyTwoRK( self.actuation.nu, crocoddyl.RKType.four) elif control == 'rk3': self.control = crocoddyl.ControlParametrizationModelPolyTwoRK( self.actuation.nu, crocoddyl.RKType.three) else: self.control = crocoddyl.ControlParametrizationModelPolyZero( self.actuation.nu) # Defining default state q0 = self.rmodel.referenceConfigurations["half_sitting"] self.rmodel.defaultState = np.concatenate( [q0, np.zeros(self.rmodel.nv)]) self.firstStep = True # Defining the friction coefficient and normal self.mu = 0.7 self.Rsurf = np.eye(3)
import crocoddyl import pinocchio import example_robot_data from test_utils import NUMDIFF_MODIFIER, assertNumDiff # Create robot model and data ROBOT_MODEL = example_robot_data.loadICub().model ROBOT_DATA = ROBOT_MODEL.createData() # Create differential action model and data; link the contact data ROBOT_STATE = crocoddyl.StateMultibody(ROBOT_MODEL) ACTUATION = crocoddyl.ActuationModelFloatingBase(ROBOT_STATE) IMPULSES = crocoddyl.ImpulseModelMultiple(ROBOT_STATE) IMPULSE_6D = crocoddyl.ImpulseModel6D(ROBOT_STATE, ROBOT_MODEL.getFrameId('r_sole')) IMPULSE_3D = crocoddyl.ImpulseModel3D(ROBOT_STATE, ROBOT_MODEL.getFrameId('l_sole')) IMPULSES.addImpulse("r_sole_impulse", IMPULSE_6D) IMPULSES.addImpulse("l_sole_impulse", IMPULSE_3D) COSTS = crocoddyl.CostModelSum(ROBOT_STATE, 0) COSTS.addCost("impulse_com", crocoddyl.CostModelImpulseCoM(ROBOT_STATE), 1.) MODEL = crocoddyl.ActionModelImpulseFwdDynamics(ROBOT_STATE, IMPULSES, COSTS, 0., 0., True) DATA = MODEL.createData() # Created DAM numdiff MODEL_ND = crocoddyl.ActionModelNumDiff(MODEL) MODEL_ND.disturbance *= 10 dnum = MODEL_ND.createData()
import pinocchio WITHDISPLAY = 'display' in sys.argv or 'CROCODDYL_DISPLAY' in os.environ WITHPLOT = 'plot' in sys.argv or 'CROCODDYL_PLOT' in os.environ # Load robot robot = example_robot_data.load('talos') rmodel = robot.model lims = rmodel.effortLimit # lims[19:] *= 0.5 # reduced artificially the torque limits rmodel.effortLimit = lims # Create data structures rdata = rmodel.createData() state = crocoddyl.StateMultibody(rmodel) actuation = crocoddyl.ActuationModelFloatingBase(state) # Set integration time DT = 5e-2 T = 60 target = np.array([0.4, 0, 1.2]) # Initialize reference state, target and reference CoM rightFoot = 'right_sole_link' leftFoot = 'left_sole_link' endEffector = 'gripper_left_joint' endEffectorId = rmodel.getFrameId(endEffector) rightFootId = rmodel.getFrameId(rightFoot) leftFootId = rmodel.getFrameId(leftFoot) q0 = rmodel.referenceConfigurations["half_sitting"] x0 = np.concatenate([q0, np.zeros(rmodel.nv)])
class TalosFloatingBaseActuationTest(ActuationModelAbstractTestCase): STATE = crocoddyl.StateMultibody(example_robot_data.loadTalos().model) ACTUATION = crocoddyl.ActuationModelFloatingBase(STATE) ACTUATION_DER = FreeFloatingActuationDerived(STATE)
class FloatingBaseActuationTest(ActuationModelAbstractTestCase): STATE = crocoddyl.StateMultibody(pinocchio.buildSampleModelHumanoidRandom()) ACTUATION = crocoddyl.ActuationModelFloatingBase(STATE) ACTUATION_DER = FreeFloatingActuationDerived(STATE)