class FrameVelocityCostSumTest(CostModelSumTestCase):
ROBOT_MODEL = example_robot_data.loadICub().model
ROBOT_STATE = crocoddyl.StateMultibody(ROBOT_MODEL)
vref = crocoddyl.FrameMotion(ROBOT_MODEL.getFrameId('r_sole'),
pinocchio.Motion.Random())
COST = crocoddyl.CostModelFrameVelocity(ROBOT_STATE, vref)
class Impulse6DTest(ImpulseModelAbstractTestCase):
ROBOT_MODEL = example_robot_data.loadICub().model
ROBOT_STATE = crocoddyl.StateMultibody(ROBOT_MODEL)
frame = ROBOT_MODEL.getFrameId('r_sole')
IMPULSE = crocoddyl.ImpulseModel6D(ROBOT_STATE, frame)
IMPULSE_DER = Impulse6DDerived(ROBOT_STATE, frame)
class FrameTranslationCostSumTest(CostModelSumTestCase):
ROBOT_MODEL = example_robot_data.loadICub().model
ROBOT_STATE = crocoddyl.StateMultibody(ROBOT_MODEL)
xref = crocoddyl.FrameTranslation(ROBOT_MODEL.getFrameId('r_sole'),
pinocchio.utils.rand(3))
COST = crocoddyl.CostModelFrameTranslation(ROBOT_STATE, xref)
class FrameRotationCostSumTest(CostModelSumTestCase):
ROBOT_MODEL = example_robot_data.loadICub().model
ROBOT_STATE = crocoddyl.StateMultibody(ROBOT_MODEL)
Rref = crocoddyl.FrameRotation(ROBOT_MODEL.getFrameId('r_sole'),
pinocchio.SE3.Random().rotation)
COST = crocoddyl.CostModelFrameRotation(ROBOT_STATE, Rref)
class CoMPositionCostTest(CostModelAbstractTestCase):
ROBOT_MODEL = example_robot_data.loadICub().model
ROBOT_STATE = crocoddyl.StateMultibody(ROBOT_MODEL)
cref = pinocchio.utils.rand(3)
COST = crocoddyl.CostModelCoMPosition(ROBOT_STATE, cref)
COST_DER = CoMPositionCostModelDerived(ROBOT_STATE, cref=cref)
class FramePlacementCostTest(CostModelAbstractTestCase):
ROBOT_MODEL = example_robot_data.loadICub().model
ROBOT_STATE = crocoddyl.StateMultibody(ROBOT_MODEL)
Mref = crocoddyl.FramePlacement(ROBOT_MODEL.getFrameId('r_sole'),
pinocchio.SE3.Random())
COST = crocoddyl.CostModelFramePlacement(ROBOT_STATE, Mref)
COST_DER = FramePlacementCostModelDerived(ROBOT_STATE, Mref=Mref)
class Contact6DTest(ContactModelAbstractTestCase):
ROBOT_MODEL = example_robot_data.loadICub().model
ROBOT_STATE = crocoddyl.StateMultibody(ROBOT_MODEL)
gains = pinocchio.utils.rand(2)
Mref = crocoddyl.FramePlacement(ROBOT_MODEL.getFrameId('r_sole'),
pinocchio.SE3.Random())
CONTACT = crocoddyl.ContactModel6D(ROBOT_STATE, Mref, gains)
CONTACT_DER = Contact6DDerived(ROBOT_STATE, Mref, gains)
class Impulse6DMultipleTest(ImpulseModelMultipleAbstractTestCase):
ROBOT_MODEL = example_robot_data.loadICub().model
ROBOT_STATE = crocoddyl.StateMultibody(ROBOT_MODEL)
gains = pinocchio.utils.rand(2)
IMPULSES = collections.OrderedDict(
sorted({
'l_sole': crocoddyl.ImpulseModel6D(ROBOT_STATE, ROBOT_MODEL.getFrameId('l_sole')),
'r_sole': crocoddyl.ImpulseModel6D(ROBOT_STATE, ROBOT_MODEL.getFrameId('r_sole'))
}.items()))
class Contact6DMultipleTest(ContactModelMultipleAbstractTestCase):
ROBOT_MODEL = example_robot_data.loadICub().model
ROBOT_STATE = crocoddyl.StateMultibody(ROBOT_MODEL)
gains = pinocchio.utils.rand(2)
CONTACTS = collections.OrderedDict(
sorted({
'l_foot':
crocoddyl.ContactModel6D(
ROBOT_STATE,
crocoddyl.FramePlacement(ROBOT_MODEL.getFrameId('l_sole'),
pinocchio.SE3.Random()), gains),
'r_foot':
crocoddyl.ContactModel6D(
ROBOT_STATE,
crocoddyl.FramePlacement(ROBOT_MODEL.getFrameId('r_sole'),
pinocchio.SE3.Random()), gains)
}.items()))
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()
class ControlCostSumTest(CostModelSumTestCase):
ROBOT_MODEL = example_robot_data.loadICub().model
ROBOT_STATE = crocoddyl.StateMultibody(ROBOT_MODEL)
COST = crocoddyl.CostModelControl(ROBOT_STATE)
class ControlCostTest(CostModelAbstractTestCase):
ROBOT_MODEL = example_robot_data.loadICub().model
ROBOT_STATE = crocoddyl.StateMultibody(ROBOT_MODEL)
COST = crocoddyl.CostModelControl(ROBOT_STATE)
COST_DER = ControlCostModelDerived(ROBOT_STATE)
class ICubTest(RobotTestCase):
RobotTestCase.ROBOT = example_robot_data.loadICub(reduced=False)
RobotTestCase.NQ = 39
RobotTestCase.NV = 38
