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)
