Python loadICub Examples

Example #1

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)

Example #2

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)

Example #3

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)

Example #4

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)

Example #5

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)

Example #6

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)

Example #7

File: test_contacts.py Project: lyltc1/crocoddyl

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)

Example #8

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()))

Example #9

File: test_contacts.py Project: lyltc1/crocoddyl

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()))

Example #10

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()

Example #11

class ControlCostSumTest(CostModelSumTestCase):
    ROBOT_MODEL = example_robot_data.loadICub().model
    ROBOT_STATE = crocoddyl.StateMultibody(ROBOT_MODEL)

    COST = crocoddyl.CostModelControl(ROBOT_STATE)

Example #12

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)

Example #13

class ICubTest(RobotTestCase):
    RobotTestCase.ROBOT = example_robot_data.loadICub(reduced=False)
    RobotTestCase.NQ = 39
    RobotTestCase.NV = 38