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)