class TalosArmFreeFwdDynamicsWithArmatureTest(ActionModelAbstractTestCase):
ROBOT_MODEL = example_robot_data.load('talos_arm').model
STATE = crocoddyl.StateMultibody(ROBOT_MODEL)
ACTUATION = crocoddyl.ActuationModelFull(STATE)
COST_SUM = crocoddyl.CostModelSum(STATE)
COST_SUM.addCost(
'gripperPose',
crocoddyl.CostModelResidual(
STATE,
crocoddyl.ResidualModelFramePlacement(
STATE, ROBOT_MODEL.getFrameId("gripper_left_joint"),
pinocchio.SE3.Random())), 1e-3)
COST_SUM.addCost(
"xReg",
crocoddyl.CostModelResidual(STATE,
crocoddyl.ResidualModelState(STATE)), 1e-7)
COST_SUM.addCost(
"uReg",
crocoddyl.CostModelResidual(STATE,
crocoddyl.ResidualModelControl(STATE)),
1e-7)
MODEL = crocoddyl.DifferentialActionModelFreeFwdDynamics(
STATE, ACTUATION, COST_SUM)
MODEL_DER = DifferentialFreeFwdDynamicsModelDerived(
STATE, ACTUATION, COST_SUM)
MODEL.armature = 0.1 * np.ones(ROBOT_MODEL.nv)
MODEL_DER.set_armature(0.1 * np.ones(ROBOT_MODEL.nv))
class TalosArmIntegratedRK4Test(ActionModelAbstractTestCase):
ROBOT_MODEL = example_robot_data.load('talos_arm').model
STATE = crocoddyl.StateMultibody(ROBOT_MODEL)
ACTUATION = crocoddyl.ActuationModelFull(STATE)
COST_SUM = crocoddyl.CostModelSum(STATE)
COST_SUM.addCost(
'gripperPose',
crocoddyl.CostModelResidual(
STATE,
crocoddyl.ResidualModelFramePlacement(
STATE, ROBOT_MODEL.getFrameId("gripper_left_joint"),
pinocchio.SE3.Random())), 1e-3)
COST_SUM.addCost(
"xReg",
crocoddyl.CostModelResidual(STATE,
crocoddyl.ResidualModelState(STATE)), 1e-7)
COST_SUM.addCost(
"uReg",
crocoddyl.CostModelResidual(STATE,
crocoddyl.ResidualModelControl(STATE)),
1e-7)
DIFFERENTIAL = crocoddyl.DifferentialActionModelFreeFwdDynamics(
STATE, ACTUATION, COST_SUM)
MODEL = crocoddyl.IntegratedActionModelRK(DIFFERENTIAL,
crocoddyl.RKType.four, 1e-3)
MODEL_DER = IntegratedActionModelRK4Derived(DIFFERENTIAL, 1e-3)
def createImpulseModel(self, supportFootIds, swingFootTask, JMinvJt_damping=1e-12, r_coeff=0.0):
""" Action model for impulse models.
An impulse model consists of describing the impulse dynamics against a set of contacts.
:param supportFootIds: Ids of the constrained feet
:param swingFootTask: swinging foot task
:return impulse action model
"""
# Creating a 3D multi-contact model, and then including the supporting foot
impulseModel = crocoddyl.ImpulseModelMultiple(self.state)
for i in supportFootIds:
supportContactModel = crocoddyl.ImpulseModel3D(self.state, i)
impulseModel.addImpulse(self.rmodel.frames[i].name + "_impulse", supportContactModel)
# Creating the cost model for a contact phase
costModel = crocoddyl.CostModelSum(self.state, 0)
if swingFootTask is not None:
for i in swingFootTask:
frameTranslationResidual = crocoddyl.ResidualModelFrameTranslation(self.state, i[0], i[1].translation,
0)
footTrack = crocoddyl.CostModelResidual(self.state, frameTranslationResidual)
costModel.addCost(self.rmodel.frames[i[0]].name + "_footTrack", footTrack, 1e7)
stateWeights = np.array([1.] * 6 + [10.] * (self.rmodel.nv - 6) + [10.] * self.rmodel.nv)
stateResidual = crocoddyl.ResidualModelState(self.state, self.rmodel.defaultState, 0)
stateActivation = crocoddyl.ActivationModelWeightedQuad(stateWeights**2)
stateReg = crocoddyl.CostModelResidual(self.state, stateActivation, stateResidual)
costModel.addCost("stateReg", stateReg, 1e1)
# Creating the action model for the KKT dynamics with simpletic Euler
# integration scheme
model = crocoddyl.ActionModelImpulseFwdDynamics(self.state, impulseModel, costModel)
model.JMinvJt_damping = JMinvJt_damping
model.r_coeff = r_coeff
return model
def createPseudoImpulseModel(self, supportFootIds, swingFootTask):
""" Action model for pseudo-impulse models.
A pseudo-impulse model consists of adding high-penalty cost for the contact velocities.
:param supportFootIds: Ids of the constrained feet
:param swingFootTask: swinging foot task
:return pseudo-impulse differential action model
"""
# Creating a 3D multi-contact model, and then including the supporting
# foot
nu = self.actuation.nu
contactModel = crocoddyl.ContactModelMultiple(self.state, nu)
for i in supportFootIds:
supportContactModel = crocoddyl.ContactModel3D(self.state, i, np.array([0., 0., 0.]), nu,
np.array([0., 50.]))
contactModel.addContact(self.rmodel.frames[i].name + "_contact", supportContactModel)
# Creating the cost model for a contact phase
costModel = crocoddyl.CostModelSum(self.state, nu)
for i in supportFootIds:
cone = crocoddyl.FrictionCone(self.Rsurf, self.mu, 4, False)
coneResidual = crocoddyl.ResidualModelContactFrictionCone(self.state, i, cone, nu)
coneActivation = crocoddyl.ActivationModelQuadraticBarrier(crocoddyl.ActivationBounds(cone.lb, cone.ub))
frictionCone = crocoddyl.CostModelResidual(self.state, coneActivation, coneResidual)
costModel.addCost(self.rmodel.frames[i].name + "_frictionCone", frictionCone, 1e1)
if swingFootTask is not None:
for i in swingFootTask:
frameTranslationResidual = crocoddyl.ResidualModelFrameTranslation(self.state, i[0], i[1].translation,
nu)
frameVelocityResidual = crocoddyl.ResidualModelFrameVelocity(self.state, i[0], pinocchio.Motion.Zero(),
pinocchio.LOCAL, nu)
footTrack = crocoddyl.CostModelResidual(self.state, frameTranslationResidual)
impulseFootVelCost = crocoddyl.CostModelResidual(self.state, frameVelocityResidual)
costModel.addCost(self.rmodel.frames[i[0]].name + "_footTrack", footTrack, 1e7)
costModel.addCost(self.rmodel.frames[i[0]].name + "_impulseVel", impulseFootVelCost, 1e6)
stateWeights = np.array([0.] * 3 + [500.] * 3 + [0.01] * (self.rmodel.nv - 6) + [10.] * self.rmodel.nv)
stateResidual = crocoddyl.ResidualModelState(self.state, self.rmodel.defaultState, nu)
stateActivation = crocoddyl.ActivationModelWeightedQuad(stateWeights**2)
ctrlResidual = crocoddyl.ResidualModelControl(self.state, nu)
stateReg = crocoddyl.CostModelResidual(self.state, stateActivation, stateResidual)
ctrlReg = crocoddyl.CostModelResidual(self.state, ctrlResidual)
costModel.addCost("stateReg", stateReg, 1e1)
costModel.addCost("ctrlReg", ctrlReg, 1e-3)
# Creating the action model for the KKT dynamics with simpletic Euler
# integration scheme
dmodel = crocoddyl.DifferentialActionModelContactFwdDynamics(self.state, self.actuation, contactModel,
costModel, 0., True)
if self.integrator == 'euler':
model = crocoddyl.IntegratedActionModelEuler(dmodel, 0.)
elif self.integrator == 'rk4':
model = crocoddyl.IntegratedActionModelRK(dmodel, crocoddyl.RKType.four, 0.)
elif self.integrator == 'rk3':
model = crocoddyl.IntegratedActionModelRK(dmodel, crocoddyl.RKType.three, 0.)
elif self.integrator == 'rk2':
model = crocoddyl.IntegratedActionModelRK(dmodel, crocoddyl.RKType.two, 0.)
else:
model = crocoddyl.IntegratedActionModelEuler(dmodel, 0.)
return model
class TalosArmShootingTest(ShootingProblemTestCase):
ROBOT_MODEL = example_robot_data.load('talos_arm').model
STATE = crocoddyl.StateMultibody(ROBOT_MODEL)
ACTUATION = crocoddyl.ActuationModelFull(STATE)
COST_SUM = crocoddyl.CostModelSum(STATE)
COST_SUM.addCost(
'gripperPose',
crocoddyl.CostModelResidual(
STATE,
crocoddyl.ResidualModelFramePlacement(
STATE, ROBOT_MODEL.getFrameId("gripper_left_joint"),
pinocchio.SE3.Random())), 1e-3)
COST_SUM.addCost(
"xReg",
crocoddyl.CostModelResidual(STATE,
crocoddyl.ResidualModelState(STATE)), 1e-7)
COST_SUM.addCost(
"uReg",
crocoddyl.CostModelResidual(STATE,
crocoddyl.ResidualModelControl(STATE)),
1e-7)
DIFF_MODEL = crocoddyl.DifferentialActionModelFreeFwdDynamics(
STATE, ACTUATION, COST_SUM)
DIFF_MODEL_DER = DifferentialFreeFwdDynamicsModelDerived(
STATE, ACTUATION, COST_SUM)
MODEL = crocoddyl.IntegratedActionModelEuler(DIFF_MODEL, 1e-3)
MODEL_DER = crocoddyl.IntegratedActionModelEuler(DIFF_MODEL_DER, 1e-3)
class AnymalFreeFwdDynamicsTest(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)
MODEL = crocoddyl.DifferentialActionModelFreeFwdDynamics(STATE, ACTUATION, COST_SUM)
MODEL_DER = DifferentialFreeFwdDynamicsModelDerived(STATE, ACTUATION, COST_SUM)
class FramePlacementCostSumTest(CostModelSumTestCase):
ROBOT_MODEL = example_robot_data.load('icub_reduced').model
ROBOT_STATE = crocoddyl.StateMultibody(ROBOT_MODEL)
COST = crocoddyl.CostModelResidual(
ROBOT_STATE,
crocoddyl.ResidualModelFramePlacement(ROBOT_STATE, ROBOT_MODEL.getFrameId('r_sole'), pinocchio.SE3.Random()))
class CoMPositionCostTest(CostModelAbstractTestCase):
ROBOT_MODEL = example_robot_data.load('icub_reduced').model
ROBOT_STATE = crocoddyl.StateMultibody(ROBOT_MODEL)
cref = pinocchio.utils.rand(3)
COST = crocoddyl.CostModelResidual(ROBOT_STATE, crocoddyl.ResidualModelCoMPosition(ROBOT_STATE, cref))
COST_DER = CoMPositionCostModelDerived(ROBOT_STATE, cref=cref)
class FrameRotationCostTest(CostModelAbstractTestCase):
ROBOT_MODEL = example_robot_data.load('icub_reduced').model
ROBOT_STATE = crocoddyl.StateMultibody(ROBOT_MODEL)
Rref = pinocchio.SE3.Random().rotation
COST = crocoddyl.CostModelResidual(
ROBOT_STATE, crocoddyl.ResidualModelFrameRotation(ROBOT_STATE, ROBOT_MODEL.getFrameId('r_sole'), Rref))
COST_DER = FrameRotationCostModelDerived(ROBOT_STATE, frame_id=ROBOT_MODEL.getFrameId('r_sole'), rotation=Rref)
class FrameTranslationCostSumTest(CostModelSumTestCase):
ROBOT_MODEL = example_robot_data.load('icub_reduced').model
ROBOT_STATE = crocoddyl.StateMultibody(ROBOT_MODEL)
COST = crocoddyl.CostModelResidual(
ROBOT_STATE,
crocoddyl.ResidualModelFrameTranslation(ROBOT_STATE, ROBOT_MODEL.getFrameId('r_sole'),
pinocchio.utils.rand(3)))
class FrameVelocityCostSumTest(CostModelSumTestCase):
ROBOT_MODEL = example_robot_data.load('icub_reduced').model
ROBOT_STATE = crocoddyl.StateMultibody(ROBOT_MODEL)
COST = crocoddyl.CostModelResidual(
ROBOT_STATE,
crocoddyl.ResidualModelFrameVelocity(ROBOT_STATE, ROBOT_MODEL.getFrameId('r_sole'), pinocchio.Motion.Random(),
pinocchio.LOCAL))
class FrameVelocityCostTest(CostModelAbstractTestCase):
ROBOT_MODEL = example_robot_data.load('icub_reduced').model
ROBOT_STATE = crocoddyl.StateMultibody(ROBOT_MODEL)
vref = pinocchio.Motion.Random()
COST = crocoddyl.CostModelResidual(
ROBOT_STATE,
crocoddyl.ResidualModelFrameVelocity(ROBOT_STATE, ROBOT_MODEL.getFrameId('r_sole'), vref, pinocchio.LOCAL))
COST_DER = FrameVelocityCostModelDerived(ROBOT_STATE, frame_id=ROBOT_MODEL.getFrameId('r_sole'), velocity=vref)
class FrameTranslationCostTest(CostModelAbstractTestCase):
ROBOT_MODEL = example_robot_data.load('icub_reduced').model
ROBOT_STATE = crocoddyl.StateMultibody(ROBOT_MODEL)
xref = pinocchio.utils.rand(3)
COST = crocoddyl.CostModelResidual(
ROBOT_STATE, crocoddyl.ResidualModelFrameTranslation(ROBOT_STATE, ROBOT_MODEL.getFrameId('r_sole'), xref))
COST_DER = FrameTranslationCostModelDerived(ROBOT_STATE,
frame_id=ROBOT_MODEL.getFrameId('r_sole'),
translation=xref)
# Cost for self-collision
maxfloat = sys.float_info.max
xlb = np.concatenate([
-maxfloat * np.ones(6), # dimension of the SE(3) manifold
rmodel.lowerPositionLimit[7:],
-maxfloat * np.ones(state.nv)
])
xub = np.concatenate([
maxfloat * np.ones(6), # dimension of the SE(3) manifold
rmodel.upperPositionLimit[7:],
maxfloat * np.ones(state.nv)
])
bounds = crocoddyl.ActivationBounds(xlb, xub, 1.)
xLimitResidual = crocoddyl.ResidualModelState(state, x0, actuation.nu)
xLimitActivation = crocoddyl.ActivationModelQuadraticBarrier(bounds)
limitCost = crocoddyl.CostModelResidual(state, xLimitActivation,
xLimitResidual)
# Cost for state and control
xResidual = crocoddyl.ResidualModelState(state, x0, actuation.nu)
xActivation = crocoddyl.ActivationModelWeightedQuad(
np.array([0] * 3 + [10.] * 3 + [0.01] * (state.nv - 6) +
[10] * state.nv)**2)
uResidual = crocoddyl.ResidualModelControl(state, actuation.nu)
xTActivation = crocoddyl.ActivationModelWeightedQuad(
np.array([0] * 3 + [10.] * 3 + [0.01] * (state.nv - 6) +
[100] * state.nv)**2)
xRegCost = crocoddyl.CostModelResidual(state, xActivation, xResidual)
uRegCost = crocoddyl.CostModelResidual(state, uResidual)
xRegTermCost = crocoddyl.CostModelResidual(state, xTActivation, xResidual)
# Cost for target reaching
def createPseudoImpulseModel(self, supportFootIds, swingFootTask):
""" Action model for pseudo-impulse models.
A pseudo-impulse model consists of adding high-penalty cost for the contact velocities.
:param swingFootTask: swinging foot task
:return pseudo-impulse differential action model
"""
# Creating a 6D multi-contact model, and then including the supporting
# foot
nu = self.actuation.nu
contactModel = crocoddyl.ContactModelMultiple(self.state, nu)
for i in supportFootIds:
supportContactModel = crocoddyl.ContactModel6D(
self.state, i, pinocchio.SE3.Identity(), nu, np.array([0.,
0.]))
contactModel.addContact(self.rmodel.frames[i].name + "_contact",
supportContactModel)
# Creating the cost model for a contact phase
costModel = crocoddyl.CostModelSum(self.state, nu)
for i in supportFootIds:
cone = crocoddyl.WrenchCone(self.Rsurf, self.mu,
np.array([0.1, 0.05]))
wrenchResidual = crocoddyl.ResidualModelContactWrenchCone(
self.state, i, cone, nu)
wrenchActivation = crocoddyl.ActivationModelQuadraticBarrier(
crocoddyl.ActivationBounds(cone.lb, cone.ub))
wrenchCone = crocoddyl.CostModelResidual(self.state,
wrenchActivation,
wrenchResidual)
costModel.addCost(self.rmodel.frames[i].name + "_wrenchCone",
wrenchCone, 1e1)
if swingFootTask is not None:
for i in swingFootTask:
framePlacementResidual = crocoddyl.ResidualModelFramePlacement(
self.state, i[0], i[1], nu)
frameVelocityResidual = crocoddyl.ResidualModelFrameVelocity(
self.state, i[0], pinocchio.Motion.Zero(), pinocchio.LOCAL,
nu)
footTrack = crocoddyl.CostModelResidual(
self.state, framePlacementResidual)
impulseFootVelCost = crocoddyl.CostModelResidual(
self.state, frameVelocityResidual)
costModel.addCost(self.rmodel.frames[i[0]].name + "_footTrack",
footTrack, 1e8)
costModel.addCost(
self.rmodel.frames[i[0]].name + "_impulseVel",
impulseFootVelCost, 1e6)
stateWeights = np.array([0.] * 3 + [500.] * 3 + [0.01] *
(self.state.nv - 6) + [10] * self.state.nv)
stateResidual = crocoddyl.ResidualModelState(self.state,
self.rmodel.defaultState,
nu)
stateActivation = crocoddyl.ActivationModelWeightedQuad(
stateWeights**2)
ctrlResidual = crocoddyl.ResidualModelControl(self.state, nu)
stateReg = crocoddyl.CostModelResidual(self.state, stateActivation,
stateResidual)
ctrlReg = crocoddyl.CostModelResidual(self.state, ctrlResidual)
costModel.addCost("stateReg", stateReg, 1e1)
costModel.addCost("ctrlReg", ctrlReg, 1e-3)
# Creating the action model for the KKT dynamics with simpletic Euler
# integration scheme
dmodel = crocoddyl.DifferentialActionModelContactFwdDynamics(
self.state, self.actuation, contactModel, costModel, 0., True)
model = crocoddyl.IntegratedActionModelEuler(dmodel, 0.)
return model
def createSwingFootModel(self,
timeStep,
supportFootIds,
comTask=None,
swingFootTask=None):
""" Action model for a swing foot phase.
:param timeStep: step duration of the action model
:param supportFootIds: Ids of the constrained feet
:param comTask: CoM task
:param swingFootTask: swinging foot task
:return action model for a swing foot phase
"""
# Creating a 6D multi-contact model, and then including the supporting
# foot
nu = self.actuation.nu
contactModel = crocoddyl.ContactModelMultiple(self.state, nu)
for i in supportFootIds:
supportContactModel = \
crocoddyl.ContactModel6D(self.state, i,
pinocchio.SE3.Identity(),
nu, np.array([0., 0.]))
contactModel.addContact(self.rmodel.frames[i].name + "_contact",
supportContactModel)
# Creating the cost model for a contact phase
costModel = crocoddyl.CostModelSum(self.state, nu)
if isinstance(comTask, np.ndarray):
comResidual = crocoddyl.ResidualModelCoMPosition(
self.state, comTask, nu)
comTrack = crocoddyl.CostModelResidual(self.state, comResidual)
costModel.addCost("comTrack", comTrack, 1e6)
for i in supportFootIds:
cone = crocoddyl.WrenchCone(self.Rsurf, self.mu,
np.array([0.1, 0.05]))
wrenchResidual = crocoddyl.ResidualModelContactWrenchCone(
self.state, i, cone, nu)
wrenchActivation = crocoddyl.ActivationModelQuadraticBarrier(
crocoddyl.ActivationBounds(cone.lb, cone.ub))
wrenchCone = crocoddyl.CostModelResidual(self.state,
wrenchActivation,
wrenchResidual)
costModel.addCost(self.rmodel.frames[i].name + "_wrenchCone",
wrenchCone, 1e1)
if swingFootTask is not None:
for i in swingFootTask:
framePlacementResidual = crocoddyl.ResidualModelFramePlacement(
self.state, i[0], i[1], nu)
footTrack = crocoddyl.CostModelResidual(
self.state, framePlacementResidual)
costModel.addCost(self.rmodel.frames[i[0]].name + "_footTrack",
footTrack, 1e6)
stateWeights = np.array([0] * 3 + [500.] * 3 + [0.01] *
(self.state.nv - 6) + [10] * self.state.nv)
stateResidual = crocoddyl.ResidualModelState(self.state,
self.rmodel.defaultState,
nu)
stateActivation = crocoddyl.ActivationModelWeightedQuad(
stateWeights**2)
ctrlResidual = crocoddyl.ResidualModelControl(self.state, nu)
stateReg = crocoddyl.CostModelResidual(self.state, stateActivation,
stateResidual)
ctrlReg = crocoddyl.CostModelResidual(self.state, ctrlResidual)
costModel.addCost("stateReg", stateReg, 1e1)
costModel.addCost("ctrlReg", ctrlReg, 1e-1)
# Creating the action model for the KKT dynamics with simpletic Euler
# integration scheme
dmodel = crocoddyl.DifferentialActionModelContactFwdDynamics(
self.state, self.actuation, contactModel, costModel, 0., True)
model = crocoddyl.IntegratedActionModelEuler(dmodel, timeStep)
return model
class StateCostTest(CostModelAbstractTestCase):
ROBOT_MODEL = example_robot_data.load('icub_reduced').model
ROBOT_STATE = crocoddyl.StateMultibody(ROBOT_MODEL)
COST = crocoddyl.CostModelResidual(ROBOT_STATE, crocoddyl.ResidualModelState(ROBOT_STATE))
COST_DER = StateCostModelDerived(ROBOT_STATE)
class ControlCostSumTest(CostModelSumTestCase):
ROBOT_MODEL = example_robot_data.load('icub_reduced').model
ROBOT_STATE = crocoddyl.StateMultibody(ROBOT_MODEL)
COST = crocoddyl.CostModelResidual(ROBOT_STATE, crocoddyl.ResidualModelControl(ROBOT_STATE))
def createSwingFootModel(self,
timeStep,
supportFootIds,
comTask=None,
swingFootTask=None):
""" Action model for a swing foot phase.
:param timeStep: step duration of the action model
:param supportFootIds: Ids of the constrained feet
:param comTask: CoM task
:param swingFootTask: swinging foot task
:return action model for a swing foot phase
"""
# Creating a 3D multi-contact model, and then including the supporting
# foot
nu = self.actuation.nu
contactModel = crocoddyl.ContactModelMultiple(self.state, nu)
for i in supportFootIds:
supportContactModel = crocoddyl.ContactModel3D(
self.state, i, np.array([0., 0., 0.]), nu, np.array([0., 50.]))
contactModel.addContact(self.rmodel.frames[i].name + "_contact",
supportContactModel)
# Creating the cost model for a contact phase
costModel = crocoddyl.CostModelSum(self.state, nu)
if isinstance(comTask, np.ndarray):
comResidual = crocoddyl.ResidualModelCoMPosition(
self.state, comTask, nu)
comTrack = crocoddyl.CostModelResidual(self.state, comResidual)
costModel.addCost("comTrack", comTrack, 1e6)
for i in supportFootIds:
cone = crocoddyl.FrictionCone(self.Rsurf, self.mu, 4, False)
coneResidual = crocoddyl.ResidualModelContactFrictionCone(
self.state, i, cone, nu)
coneActivation = crocoddyl.ActivationModelQuadraticBarrier(
crocoddyl.ActivationBounds(cone.lb, cone.ub))
frictionCone = crocoddyl.CostModelResidual(self.state,
coneActivation,
coneResidual)
costModel.addCost(self.rmodel.frames[i].name + "_frictionCone",
frictionCone, 1e1)
if swingFootTask is not None:
for i in swingFootTask:
frameTranslationResidual = crocoddyl.ResidualModelFrameTranslation(
self.state, i[0], i[1].translation, nu)
footTrack = crocoddyl.CostModelResidual(
self.state, frameTranslationResidual)
costModel.addCost(self.rmodel.frames[i[0]].name + "_footTrack",
footTrack, 1e6)
stateWeights = np.array([0.] * 3 + [500.] * 3 + [0.01] *
(self.rmodel.nv - 6) + [10.] * 6 + [1.] *
(self.rmodel.nv - 6))
stateResidual = crocoddyl.ResidualModelState(self.state,
self.rmodel.defaultState,
nu)
stateActivation = crocoddyl.ActivationModelWeightedQuad(
stateWeights**2)
ctrlResidual = crocoddyl.ResidualModelControl(self.state, nu)
stateReg = crocoddyl.CostModelResidual(self.state, stateActivation,
stateResidual)
ctrlReg = crocoddyl.CostModelResidual(self.state, ctrlResidual)
costModel.addCost("stateReg", stateReg, 1e1)
costModel.addCost("ctrlReg", ctrlReg, 1e-1)
lb = np.concatenate([
self.state.lb[1:self.state.nv + 1], self.state.lb[-self.state.nv:]
])
ub = np.concatenate([
self.state.ub[1:self.state.nv + 1], self.state.ub[-self.state.nv:]
])
stateBoundsResidual = crocoddyl.ResidualModelState(self.state, nu)
stateBoundsActivation = crocoddyl.ActivationModelQuadraticBarrier(
crocoddyl.ActivationBounds(lb, ub))
stateBounds = crocoddyl.CostModelResidual(self.state,
stateBoundsActivation,
stateBoundsResidual)
costModel.addCost("stateBounds", stateBounds, 1e3)
# Creating the action model for the KKT dynamics with simpletic Euler
# integration scheme
dmodel = crocoddyl.DifferentialActionModelContactFwdDynamics(
self.state, self.actuation, contactModel, costModel, 0., True)
model = crocoddyl.IntegratedActionModelEuler(dmodel, timeStep)
return model
# Cost for self-collision
maxfloat = sys.float_info.max
xlb = np.concatenate([
-maxfloat * np.ones(6), # dimension of the SE(3) manifold
rmodel.lowerPositionLimit[7:],
-maxfloat * np.ones(state.nv)
])
xub = np.concatenate([
maxfloat * np.ones(6), # dimension of the SE(3) manifold
rmodel.upperPositionLimit[7:],
maxfloat * np.ones(state.nv)
])
bounds = crocoddyl.ActivationBounds(xlb, xub, 1.)
xLimitResidual = crocoddyl.ResidualModelState(state, x0, actuation.nu)
xLimitActivation = crocoddyl.ActivationModelQuadraticBarrier(bounds)
limitCost = crocoddyl.CostModelResidual(state, xLimitActivation, xLimitResidual)
# Cost for state and control
xResidual = crocoddyl.ResidualModelState(state, x0, actuation.nu)
xActivation = crocoddyl.ActivationModelWeightedQuad(
np.array([0] * 3 + [10.] * 3 + [0.01] * (state.nv - 6) + [10] * state.nv)**2)
uResidual = crocoddyl.ResidualModelControl(state, actuation.nu)
xTActivation = crocoddyl.ActivationModelWeightedQuad(
np.array([0] * 3 + [10.] * 3 + [0.01] * (state.nv - 6) + [100] * state.nv)**2)
xRegCost = crocoddyl.CostModelResidual(state, xActivation, xResidual)
uRegCost = crocoddyl.CostModelResidual(state, uResidual)
xRegTermCost = crocoddyl.CostModelResidual(state, xTActivation, xResidual)
# Cost for target reaching: hand and foot
handTrackingResidual = crocoddyl.ResidualModelFramePlacement(state, endEffectorId, pinocchio.SE3(np.eye(3), target),
actuation.nu)
# Loading the double pendulum model
pendulum = example_robot_data.load('double_pendulum')
model = pendulum.model
state = crocoddyl.StateMultibody(model)
actuation = ActuationModelDoublePendulum(state, actLink=1)
nu = actuation.nu
runningCostModel = crocoddyl.CostModelSum(state, nu)
terminalCostModel = crocoddyl.CostModelSum(state, nu)
xResidual = crocoddyl.ResidualModelState(state, state.zero(), nu)
xActivation = crocoddyl.ActivationModelQuad(state.ndx)
uResidual = crocoddyl.ResidualModelControl(state, nu)
xRegCost = crocoddyl.CostModelResidual(state, xActivation, xResidual)
uRegCost = crocoddyl.CostModelResidual(state, uResidual)
xPendCost = CostModelDoublePendulum(state, crocoddyl.ActivationModelWeightedQuad(np.array([1.] * 4 + [0.1] * 2)), nu)
dt = 1e-2
runningCostModel.addCost("uReg", uRegCost, 1e-4 / dt)
runningCostModel.addCost("xGoal", xPendCost, 1e-5 / dt)
terminalCostModel.addCost("xGoal", xPendCost, 1e4)
runningModel = crocoddyl.IntegratedActionModelEuler(
crocoddyl.DifferentialActionModelFreeFwdDynamics(state, actuation, runningCostModel), dt)
terminalModel = crocoddyl.IntegratedActionModelEuler(
crocoddyl.DifferentialActionModelFreeFwdDynamics(state, actuation, terminalCostModel), dt)
# Creating the shooting problem and the FDDP solver
# Create a cost model per the running and terminal action model.
state = crocoddyl.StateMultibody(robot_model)
runningCostModel = crocoddyl.CostModelSum(state)
terminalCostModel = crocoddyl.CostModelSum(state)
# Note that we need to include a cost model (i.e. set of cost functions) in
# order to fully define the action model for our optimal control problem.
# For this particular example, we formulate three running-cost functions:
# goal-tracking cost, state and control regularization; and one terminal-cost:
# goal cost. First, let's create the common cost functions.
framePlacementResidual = crocoddyl.ResidualModelFramePlacement(
state, robot_model.getFrameId("gripper_left_joint"),
pinocchio.SE3(np.eye(3), np.array([.0, .0, .4])))
uResidual = crocoddyl.ResidualModelControl(state)
xResidual = crocoddyl.ResidualModelControl(state)
goalTrackingCost = crocoddyl.CostModelResidual(state, framePlacementResidual)
xRegCost = crocoddyl.CostModelResidual(state, xResidual)
uRegCost = crocoddyl.CostModelResidual(state, uResidual)
# Then let's added the running and terminal cost functions
runningCostModel.addCost("gripperPose", goalTrackingCost, 1)
runningCostModel.addCost("xReg", xRegCost, 1e-4)
runningCostModel.addCost("uReg", uRegCost, 1e-4)
terminalCostModel.addCost("gripperPose", goalTrackingCost, 1)
# Next, we need to create an action model for running and terminal knots. The
# forward dynamics (computed using ABA) are implemented
# inside DifferentialActionModelFullyActuated.
actuationModel = crocoddyl.ActuationModelFull(state)
dt = 1e-3
runningModel = crocoddyl.IntegratedActionModelEuler(
state = crocoddyl.StateMultibody(robot_model)
d_cog, cf, cm, u_lim, l_lim = 0.1525, 6.6e-5, 1e-6, 5., 0.1
tau_f = np.array([[0., 0., 0., 0.], [0., 0., 0., 0.], [1., 1., 1., 1.], [0., d_cog, 0., -d_cog],
[-d_cog, 0., d_cog, 0.], [-cm / cf, cm / cf, -cm / cf, cm / cf]])
actuation = crocoddyl.ActuationModelMultiCopterBase(state, tau_f)
nu = actuation.nu
runningCostModel = crocoddyl.CostModelSum(state, nu)
terminalCostModel = crocoddyl.CostModelSum(state, nu)
# Costs
xResidual = crocoddyl.ResidualModelState(state, state.zero(), nu)
xActivation = crocoddyl.ActivationModelWeightedQuad(np.array([0.1] * 3 + [1000.] * 3 + [1000.] * robot_model.nv))
uResidual = crocoddyl.ResidualModelControl(state, nu)
xRegCost = crocoddyl.CostModelResidual(state, xActivation, xResidual)
uRegCost = crocoddyl.CostModelResidual(state, uResidual)
goalTrackingResidual = crocoddyl.ResidualModelFramePlacement(state, robot_model.getFrameId("base_link"),
pinocchio.SE3(target_quat.matrix(), target_pos), nu)
goalTrackingCost = crocoddyl.CostModelResidual(state, goalTrackingResidual)
runningCostModel.addCost("xReg", xRegCost, 1e-6)
runningCostModel.addCost("uReg", uRegCost, 1e-6)
runningCostModel.addCost("trackPose", goalTrackingCost, 1e-2)
terminalCostModel.addCost("goalPose", goalTrackingCost, 100.)
dt = 3e-2
runningModel = crocoddyl.IntegratedActionModelEuler(
crocoddyl.DifferentialActionModelFreeFwdDynamics(state, actuation, runningCostModel), dt)
terminalModel = crocoddyl.IntegratedActionModelEuler(
crocoddyl.DifferentialActionModelFreeFwdDynamics(state, actuation, terminalCostModel), dt)
target = np.array([0.4, 0., .4])
cameraTF = [2., 2.68, 0.54, 0.2, 0.62, 0.72, 0.22]
display = crocoddyl.GepettoDisplay(robot, cameraTF=cameraTF, floor=False)
robot.viewer.gui.addSphere('world/point', .05,
[1., 0., 0., 1.]) # radius = .1, RGBA=1001
robot.viewer.gui.applyConfiguration('world/point',
target.tolist() +
[0., 0., 0., 1.]) # xyz+quaternion
robot.viewer.gui.refresh()
# Create the cost functions
state = crocoddyl.StateMultibody(robot.model)
goalResidual = crocoddyl.ResidualModelFrameTranslation(
state, robot_model.getFrameId("gripper_left_joint"), target)
goalTrackingCost = crocoddyl.CostModelResidual(state, goalResidual)
xRegCost = crocoddyl.CostModelResidual(state,
crocoddyl.ResidualModelState(state))
uRegCost = crocoddyl.CostModelResidual(state,
crocoddyl.ResidualModelControl(state))
# Create cost model per each action model
runningCostModel = crocoddyl.CostModelSum(state)
terminalCostModel = crocoddyl.CostModelSum(state)
# Then let's added the running and terminal cost functions
runningCostModel.addCost("gripperPose", goalTrackingCost, 1.)
runningCostModel.addCost("stateReg", xRegCost, 5e-2)
runningCostModel.addCost("ctrlReg", uRegCost, 1e-5)
terminalCostModel.addCost("gripperPose", goalTrackingCost, 10000.)
terminalCostModel.addCost("stateReg", xRegCost, 5e-2)
