def cartpole(): # Loading the double pendulum model robot = example_robot_data.loadDoublePendulum() robot_model = robot.model state = crocoddyl.StateMultibody(robot_model) actModel = ActuationModelDoublePendulum(state, actLink=1) weights = np.array([1, 1, 1, 1] + [0.1] * 2) runningCostModel = crocoddyl.CostModelSum(state, actModel.nu) terminalCostModel = crocoddyl.CostModelSum(state, actModel.nu) xRegCost = crocoddyl.CostModelState( state, crocoddyl.ActivationModelQuad(state.ndx), state.zero(), actModel.nu) uRegCost = crocoddyl.CostModelControl(state, crocoddyl.ActivationModelQuad(1), actModel.nu) xPendCost = CostModelDoublePendulum( state, crocoddyl.ActivationModelWeightedQuad(np.matrix(weights).T), actModel.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, actModel, runningCostModel), dt) terminalModel = crocoddyl.IntegratedActionModelEuler( crocoddyl.DifferentialActionModelFreeFwdDynamics( state, actModel, terminalCostModel), dt) return runningModel, terminalModel
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
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: xref = crocoddyl.FrameTranslation(i.id, i.placement.translation) footTrack = crocoddyl.CostModelFrameTranslation(self.state, xref, 0) costModel.addCost(self.rmodel.frames[i.id].name + "_footTrack", footTrack, 1e7) stateWeights = np.array([1.] * 6 + [10.] * (self.rmodel.nv - 6) + [10.] * self.rmodel.nv) stateReg = crocoddyl.CostModelState(self.state, crocoddyl.ActivationModelWeightedQuad(stateWeights**2), self.rmodel.defaultState, 0) 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 swingFootTask: swinging foot task :return pseudo-impulse differential action model """ # Creating a 6D multi-contact model, and then including the supporting # foot contactModel = crocoddyl.ContactModelMultiple(self.state, self.actuation.nu) for i in supportFootIds: Mref = crocoddyl.FramePlacement(i, pinocchio.SE3.Identity()) supportContactModel = crocoddyl.ContactModel6D( self.state, Mref, self.actuation.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, self.actuation.nu) for i in supportFootIds: cone = crocoddyl.WrenchCone(np.identity(3), self.mu, np.array([0.1, 0.05])) wrenchCone = crocoddyl.CostModelContactWrenchCone( self.state, crocoddyl.ActivationModelQuadraticBarrier( crocoddyl.ActivationBounds(cone.lb, cone.ub)), crocoddyl.FrameWrenchCone(i, cone), self.actuation.nu) costModel.addCost(self.rmodel.frames[i].name + "_wrenchCone", wrenchCone, 1e1) if swingFootTask is not None: for i in swingFootTask: footTrack = crocoddyl.CostModelFramePlacement( self.state, i, self.actuation.nu) costModel.addCost(self.rmodel.frames[i.id].name + "_footTrack", footTrack, 1e8) footVel = crocoddyl.FrameMotion(i.id, pinocchio.Motion.Zero()) impulseFootVelCost = crocoddyl.CostModelFrameVelocity( self.state, footVel, self.actuation.nu) costModel.addCost( self.rmodel.frames[i.id].name + "_impulseVel", impulseFootVelCost, 1e6) stateWeights = np.array([0] * 3 + [500.] * 3 + [0.01] * (self.state.nv - 6) + [10] * self.state.nv) stateReg = crocoddyl.CostModelState( self.state, crocoddyl.ActivationModelWeightedQuad(stateWeights**2), self.rmodel.defaultState, self.actuation.nu) ctrlReg = crocoddyl.CostModelControl(self.state, self.actuation.nu) 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 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 contactModel = crocoddyl.ContactModelMultiple(self.state, self.actuation.nu) for i in supportFootIds: xref = crocoddyl.FrameTranslation(i, np.array([0., 0., 0.])) supportContactModel = crocoddyl.ContactModel3D( self.state, xref, self.actuation.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, self.actuation.nu) for i in supportFootIds: cone = crocoddyl.FrictionCone(self.nsurf, self.mu, 4, False) frictionCone = crocoddyl.CostModelContactFrictionCone( self.state, crocoddyl.ActivationModelQuadraticBarrier( crocoddyl.ActivationBounds(cone.lb, cone.ub)), crocoddyl.FrameFrictionCone(i, cone), self.actuation.nu) costModel.addCost(self.rmodel.frames[i].name + "_frictionCone", frictionCone, 1e1) if swingFootTask is not None: for i in swingFootTask: xref = crocoddyl.FrameTranslation(i.frame, i.oMf.translation) vref = crocoddyl.FrameMotion(i.frame, pinocchio.Motion.Zero()) footTrack = crocoddyl.CostModelFrameTranslation( self.state, xref, self.actuation.nu) impulseFootVelCost = crocoddyl.CostModelFrameVelocity( self.state, vref, self.actuation.nu) costModel.addCost( self.rmodel.frames[i.frame].name + "_footTrack", footTrack, 1e7) costModel.addCost( self.rmodel.frames[i.frame].name + "_impulseVel", impulseFootVelCost, 1e6) stateWeights = np.array([0.] * 3 + [500.] * 3 + [0.01] * (self.rmodel.nv - 6) + [10.] * self.rmodel.nv) stateReg = crocoddyl.CostModelState( self.state, crocoddyl.ActivationModelWeightedQuad(stateWeights**2), self.rmodel.defaultState, self.actuation.nu) ctrlReg = crocoddyl.CostModelControl(self.state, self.actuation.nu) 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 3D multi-contact model, and then including the supporting # foot contactModel = crocoddyl.ContactModelMultiple(self.state, self.actuation.nu) for i in supportFootIds: xref = crocoddyl.FrameTranslation(i, np.matrix([0., 0., 0.]).T) supportContactModel = crocoddyl.ContactModel3D(self.state, xref, self.actuation.nu, np.matrix([0., 50.]).T) contactModel.addContact(self.rmodel.frames[i].name + "_contact", supportContactModel) # Creating the cost model for a contact phase costModel = crocoddyl.CostModelSum(self.state, self.actuation.nu) if isinstance(comTask, np.ndarray): comTrack = crocoddyl.CostModelCoMPosition(self.state, comTask, self.actuation.nu) costModel.addCost("comTrack", comTrack, 1e6) for i in supportFootIds: cone = crocoddyl.FrictionCone(self.nsurf, self.mu, 4, False) frictionCone = crocoddyl.CostModelContactFrictionCone( self.state, crocoddyl.ActivationModelQuadraticBarrier(crocoddyl.ActivationBounds(cone.lb, cone.ub)), cone, i, self.actuation.nu) costModel.addCost(self.rmodel.frames[i].name + "_frictionCone", frictionCone, 1e1) if swingFootTask is not None: for i in swingFootTask: xref = crocoddyl.FrameTranslation(i.frame, i.oMf.translation) footTrack = crocoddyl.CostModelFrameTranslation(self.state, xref, self.actuation.nu) costModel.addCost(self.rmodel.frames[i.frame].name + "_footTrack", footTrack, 1e6) stateWeights = np.array([0.] * 3 + [500.] * 3 + [0.01] * (self.rmodel.nv - 6) + [10.] * 6 + [1.] * (self.rmodel.nv - 6)) stateReg = crocoddyl.CostModelState(self.state, crocoddyl.ActivationModelWeightedQuad(np.matrix(stateWeights**2).T), self.rmodel.defaultState, self.actuation.nu) ctrlReg = crocoddyl.CostModelControl(self.state, self.actuation.nu) costModel.addCost("stateReg", stateReg, 1e1) costModel.addCost("ctrlReg", ctrlReg, 1e-1) lb = self.state.diff(0 * self.state.lb, self.state.lb) ub = self.state.diff(0 * self.state.ub, self.state.ub) stateBounds = crocoddyl.CostModelState( self.state, crocoddyl.ActivationModelQuadraticBarrier(crocoddyl.ActivationBounds(lb, ub)), 0 * self.rmodel.defaultState, self.actuation.nu) 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
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 contactModel = crocoddyl.ContactModelMultiple(self.state, self.actuation.nu) for i in supportFootIds: Mref = crocoddyl.FramePlacement(i, pinocchio.SE3.Identity()) supportContactModel = \ crocoddyl.ContactModel6D(self.state, Mref, self.actuation.nu, np.matrix([0., 0.]).T) contactModel.addContact(self.rmodel.frames[i].name + "_contact", supportContactModel) # Creating the cost model for a contact phase costModel = crocoddyl.CostModelSum(self.state, self.actuation.nu) if isinstance(comTask, np.ndarray): comTrack = crocoddyl.CostModelCoMPosition(self.state, comTask, self.actuation.nu) costModel.addCost("comTrack", comTrack, 1e6) if swingFootTask is not None: for i in swingFootTask: footTrack = crocoddyl.CostModelFramePlacement( self.state, i, self.actuation.nu) costModel.addCost( self.rmodel.frames[i.frame].name + "_footTrack", footTrack, 1e6) stateWeights = np.array([0] * 3 + [500.] * 3 + [0.01] * (self.state.nv - 6) + [10] * self.state.nv) stateReg = crocoddyl.CostModelState( self.state, crocoddyl.ActivationModelWeightedQuad( np.matrix(stateWeights**2).T), self.rmodel.defaultState, self.actuation.nu) ctrlReg = crocoddyl.CostModelControl(self.state, self.actuation.nu) 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) model = crocoddyl.IntegratedActionModelEuler(dmodel, timeStep) return model
def createFootSwitchModel(self, supportFootIds, swingFootTask): """ Action model for a foot switch phase. :param supportFootIds: Ids of the constrained feet :param swingFootTask: swinging foot task :return action model for a foot switch phase """ # Creating a 3D multi-contact model, and then including the supporting # foot contactModel = crocoddyl.ContactModelMultiple(self.state, self.actuation.nu) for i in supportFootIds: xref = crocoddyl.FrameTranslation(i, np.matrix([0., 0., 0.]).T) supportContactModel = crocoddyl.ContactModel3D( self.state, xref, self.actuation.nu, np.matrix([0., 0.]).T) contactModel.addContact('contact_' + str(i), supportContactModel) # Creating the cost model for a contact phase costModel = crocoddyl.CostModelSum(self.state, self.actuation.nu) if swingFootTask is not None: for i in swingFootTask: xref = crocoddyl.FrameTranslation(i.frame, i.oMf.translation) footTrack = crocoddyl.CostModelFrameTranslation( self.state, xref, self.actuation.nu) costModel.addCost("footTrack_" + str(i), footTrack, 1e7) stateWeights = np.array([0] * 3 + [500.] * 3 + [0.01] * (self.rmodel.nv - 6) + [10] * self.rmodel.nv) stateReg = crocoddyl.CostModelState( self.state, crocoddyl.ActivationModelWeightedQuad( np.matrix(stateWeights**2).T), self.rmodel.defaultState, self.actuation.nu) ctrlReg = crocoddyl.CostModelControl(self.state, self.actuation.nu) costModel.addCost("stateReg", stateReg, 1e1) costModel.addCost("ctrlReg", ctrlReg, 1e-3) for i in swingFootTask: vref = crocoddyl.FrameMotion(i.frame, pinocchio.Motion.Zero()) impactFootVelCost = crocoddyl.CostModelFrameVelocity( self.state, vref, self.actuation.nu) costModel.addCost('impactVel_' + str(i.frame), impactFootVelCost, 1e6) # Creating the action model for the KKT dynamics with simpletic Euler # integration scheme dmodel = crocoddyl.DifferentialActionModelContactFwdDynamics( self.state, self.actuation, contactModel, costModel) model = crocoddyl.IntegratedActionModelEuler(dmodel, 0.) return model
def solver(starting_condition, T=30, precision=1e-9, maxiters=1000): """Solve one pendulum problem""" robot = example_robot_data.loadDoublePendulum() robot_model = robot.model state = crocoddyl.StateMultibody(robot_model) actModel = ActuationModelDoublePendulum(state, actLink=1) weights = np.array([1.5, 1.5, 1, 1] + [0.1] * 2) runningCostModel = crocoddyl.CostModelSum(state, actModel.nu) dt = 1e-2 xRegCost = crocoddyl.CostModelState( state, crocoddyl.ActivationModelQuad(state.ndx), state.zero(), actModel.nu) uRegCost = crocoddyl.CostModelControl(state, crocoddyl.ActivationModelQuad(1), actModel.nu) xPendCost = CostModelDoublePendulum( state, crocoddyl.ActivationModelWeightedQuad(weights), actModel.nu) runningCostModel.addCost("uReg", uRegCost, 1e-4 / dt) runningCostModel.addCost("xGoal", xPendCost, 1e-5 / dt) runningModel = crocoddyl.IntegratedActionModelEuler( crocoddyl.DifferentialActionModelFreeFwdDynamics( state, actModel, runningCostModel), dt) terminalCostModel = crocoddyl.CostModelSum(state, actModel.nu) terminalCostModel.addCost("xGoal", xPendCost, 1e4) terminal_model = crocoddyl.IntegratedActionModelEuler( crocoddyl.DifferentialActionModelFreeFwdDynamics( state, actModel, terminalCostModel), dt) problem = crocoddyl.ShootingProblem(starting_condition, [runningModel] * T, terminal_model) fddp = crocoddyl.SolverFDDP(problem) fddp.th_stop = precision fddp.solve([], [], maxiters)
def createImpulseModel(self, supportFootIds, swingFootTask): """ 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 6D multi-contact model, and then including the supporting foot impulseModel = crocoddyl.ImpulseModelMultiple(self.state) for i in supportFootIds: supportContactModel = crocoddyl.ImpulseModel6D(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, 1e8) stateWeights = np.array([1.] * 6 + [0.1] * (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) return model
assert (robot_model.existFrame(footName)) Pref = crocoddyl.FrameTranslation(footFrameID, target) # If also the orientation is useful for the task use # Mref = crocoddyl.FramePlacement(footFrameID, # pinocchio.SE3(R, conf.n_links * np.matrix([[np.sin(angle)], [0], [np.cos(angle)]]))) footTrackingCost = crocoddyl.CostModelFrameTranslation(state, Pref, actuation.nu) Vref = crocoddyl.FrameMotion(footFrameID, pinocchio.Motion(np.zeros(6))) footFinalVelocity = crocoddyl.CostModelFrameVelocity(state, Vref, actuation.nu) # simulating the cost on the power with a cost on the control power_act = crocoddyl.ActivationModelQuad(conf.n_links) u2 = crocoddyl.CostModelControl( state, power_act, actuation.nu) # joule dissipation cost without friction, for benchmarking stateAct = crocoddyl.ActivationModelWeightedQuad( np.concatenate([np.zeros(state.nq + 1), np.ones(state.nv - 1)])) v2 = crocoddyl.CostModelState(state, stateAct, np.zeros(state.nx), actuation.nu) joint_friction = CostModelJointFrictionSmooth(state, power_act, actuation.nu) joule_dissipation = CostModelJouleDissipation(state, power_act, actuation.nu) # PENALIZATIONS bounds = crocoddyl.ActivationBounds( np.concatenate([np.zeros(1), -1e3 * np.ones(state.nx - 1)]), 1e3 * np.ones(state.nx)) stateAct = crocoddyl.ActivationModelWeightedQuadraticBarrier( bounds, np.concatenate([np.ones(1), np.zeros(state.nx - 1)])) nonPenetration = crocoddyl.CostModelState(state, stateAct, np.zeros(state.nx), actuation.nu)
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
robot = example_robot_data.loadDoublePendulum() robot_model = robot.model state = crocoddyl.StateMultibody(robot_model) actModel = ActuationModelDoublePendulum(state, actLink=1) weights = np.array([1, 1, 1, 1] + [0.1] * 2) runningCostModel = crocoddyl.CostModelSum(state, actModel.nu) terminalCostModel = crocoddyl.CostModelSum(state, actModel.nu) xRegCost = crocoddyl.CostModelState(state, crocoddyl.ActivationModelQuad(state.ndx), state.zero(), actModel.nu) uRegCost = crocoddyl.CostModelControl(state, crocoddyl.ActivationModelQuad(1), actModel.nu) xPendCost = CostModelDoublePendulum( state, crocoddyl.ActivationModelWeightedQuad(weights), actModel.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, actModel, runningCostModel), dt) terminalModel = crocoddyl.IntegratedActionModelEuler( crocoddyl.DifferentialActionModelFreeFwdDynamics(state, actModel, terminalCostModel), dt) # Creating the shooting problem and the FDDP solver
def optimalSolution(init_states: Union[list, np.ndarray, torch.Tensor], terminal_model: crocoddyl.ActionModelAbstract = None, horizon: int = 150, precision: float = 1e-9, maxiters: int = 1000, use_fddp: bool = True): """Solve double pendulum problem with the given terminal model for the given position Parameters ---------- init_states : list or array or tensor These are the initial, starting configurations for the double pendulum terminal_model: crocoddyl.ActionModelAbstract The terminal model to be used to solve the problem horizon : int Time horizon for the running model precision : float precision for ddp.th_stop maxiters : int Maximum iterations allowed for the problem use_fddp : boolean Solve using ddp or fddp Returns -------- ddp : crocoddyl.Solverddp the optimal ddp or fddp of the prblem """ if isinstance(init_states, torch.Tensor): init_states = init_states.numpy() init_states = np.atleast_2d(init_states) solutions = [] for init_state in init_states: robot = example_robot_data.loadDoublePendulum() robot_model = robot.model state = crocoddyl.StateMultibody(robot_model) actModel = ActuationModelDoublePendulum(state, actLink=1) weights = np.array([1.5, 1.5, 1, 1] + [0.1] * 2) runningCostModel = crocoddyl.CostModelSum(state, actModel.nu) dt = 1e-2 xRegCost = crocoddyl.CostModelState( state, crocoddyl.ActivationModelQuad(state.ndx), state.zero(), actModel.nu) uRegCost = crocoddyl.CostModelControl( state, crocoddyl.ActivationModelQuad(1), actModel.nu) xPendCost = CostModelDoublePendulum( state, crocoddyl.ActivationModelWeightedQuad(weights), actModel.nu) runningCostModel.addCost("uReg", uRegCost, 1e-4 / dt) runningCostModel.addCost("xGoal", xPendCost, 1e-5 / dt) runningModel = crocoddyl.IntegratedActionModelEuler( crocoddyl.DifferentialActionModelFreeFwdDynamics( state, actModel, runningCostModel), dt) if terminal_model is None: terminalCostModel = crocoddyl.CostModelSum(state, actModel.nu) terminalCostModel.addCost("xGoal", xPendCost, 1e4) terminal_model = crocoddyl.IntegratedActionModelEuler( crocoddyl.DifferentialActionModelFreeFwdDynamics( state, actModel, terminalCostModel), dt) problem = crocoddyl.ShootingProblem(init_state, [runningModel] * horizon, terminal_model) if use_fddp: fddp = crocoddyl.SolverFDDP(problem) else: fddp = crocoddyl.SolverDDP(problem) fddp.th_stop = precision fddp.solve([], [], maxiters) solutions.append(fddp) return solutions
def solve_problem(terminal_model=None, initial_configuration=None, horizon: int = 100, precision: float = 1e-9, maxiters: int = 1000): """ Solve the problem for a given initial_position. @params: 1: terminal_model = Terminal model with neural network inside it, for the crocoddyl problem. If none, then Crocoddyl Integrated Action Model will be used as terminal model. 2: initial_configuration = initial position for the unicycle, either a list or a numpy array or a tensor. 3: horizon = Time horizon for the unicycle. Defaults to 100 4: stop = ddp.th_stop. Defaults to 1e-9 5: maxiters = maximum iterations allowed for crocoddyl.Defaults to 1000 @returns: ddp """ if isinstance(initial_configuration, list): initial_configuration = np.array(initial_configuration) elif isinstance(initial_configuration, torch.Tensor): initial_configuration = initial_configuration.numpy() # Loading the double pendulum model robot = example_robot_data.loadDoublePendulum() robot_model = robot.model state = crocoddyl.StateMultibody(robot_model) actModel = ActuationModelDoublePendulum(state, actLink=1) weights = np.array([1, 1, 1, 1] + [0.1] * 2) runningCostModel = crocoddyl.CostModelSum(state, actModel.nu) xRegCost = crocoddyl.CostModelState( state, crocoddyl.ActivationModelQuad(state.ndx), state.zero(), actModel.nu) uRegCost = crocoddyl.CostModelControl(state, crocoddyl.ActivationModelQuad(1), actModel.nu) xPendCost = CostModelDoublePendulum( state, crocoddyl.ActivationModelWeightedQuad(np.matrix(weights).T), actModel.nu) dt = 1e-2 runningCostModel.addCost("uReg", uRegCost, 1e-4 / dt) runningCostModel.addCost("xGoal", xPendCost, 1e-5 / dt) runningModel = crocoddyl.IntegratedActionModelEuler( crocoddyl.DifferentialActionModelFreeFwdDynamics( state, actModel, runningCostModel), dt) if terminal_model is None: terminalCostModel = crocoddyl.CostModelSum(state, actModel.nu) terminalCostModel.addCost("xGoal", xPendCost, 1e4) terminal_model = crocoddyl.IntegratedActionModelEuler( crocoddyl.DifferentialActionModelFreeFwdDynamics( state, actModel, terminalCostModel), dt) # Creating the shooting problem and the FDDP solver problem = crocoddyl.ShootingProblem(initial_configuration.T, [runningModel] * horizon, terminal_model) fddp = crocoddyl.SolverFDDP(problem) fddp.th_stop = precision fddp.solve([], [], maxiters) return fddp
# 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. Mref = crocoddyl.FramePlacement(FRAME_TIP, pinocchio.SE3(np.eye(3), goal)) #goalTrackingCost = crocoddyl.CostModelFramePlacement(state, Mref) pref = crocoddyl.FrameTranslation(FRAME_TIP, goal) goalTrackingCost = crocoddyl.CostModelFrameTranslation(state, pref) weights = crocoddyl.ActivationModelWeightedQuad( np.array([1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 2, 2, 2.])) xRegCost = crocoddyl.CostModelState(state, weights, robot_model.x0) uRegCost = crocoddyl.CostModelControl(state) weightsT = crocoddyl.ActivationModelWeightedQuad( np.array([.01, .01, .01, .01, .01, .01, .01, 1, 1, 1, 1, 2, 2, 2.])) xRegCostT = crocoddyl.CostModelState(state, weights, robot_model.x0) # Then let's added the running and terminal cost functions runningCostModel.addCost("gripperPose", goalTrackingCost, .001) runningCostModel.addCost("xReg", xRegCost, 1e-3) runningCostModel.addCost("uReg", uRegCost, 1e-6) terminalCostModel.addCost("gripperPose", goalTrackingCost, 10) terminalCostModel.addCost("xReg", xRegCostT, .01) # Next, we need to create an action model for running and terminal knots. The # forward dynamics (computed using ABA) are implemented
]) 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 framePlacementResidual = crocoddyl.ResidualModelFramePlacement( state, endEffectorId, pinocchio.SE3(np.eye(3), target), actuation.nu) framePlacementActivation = crocoddyl.ActivationModelWeightedQuad( np.array([1] * 3 + [0.0001] * 3)**2) goalTrackingCost = crocoddyl.CostModelResidual(state, framePlacementActivation,
target_quat = pinocchio.Quaternion(1., 0., 0., 0.) 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(
maxfloat * np.ones(6), # dimension of the SE(3) manifold rmodel.upperPositionLimit[7:], maxfloat * np.ones(state.nv) ]) bounds = crocoddyl.ActivationBounds(xlb, xub, 1.) limitCost = crocoddyl.CostModelState( state, crocoddyl.ActivationModelQuadraticBarrier(bounds), rmodel.defaultState, actuation.nu) # Cost for state and control stateWeights = np.array([0] * 3 + [10.] * 3 + [0.01] * (state.nv - 6) + [10] * state.nv) stateWeightsTerm = np.array([0] * 3 + [10.] * 3 + [0.01] * (state.nv - 6) + [100] * state.nv) xRegCost = crocoddyl.CostModelState( state, crocoddyl.ActivationModelWeightedQuad(stateWeights**2), rmodel.defaultState, actuation.nu) uRegCost = crocoddyl.CostModelControl(state, actuation.nu) xRegTermCost = crocoddyl.CostModelState( state, crocoddyl.ActivationModelWeightedQuad(stateWeightsTerm**2), rmodel.defaultState, actuation.nu) # Cost for target reaching goaltrackingWeights = np.array([1] * 3 + [0.0001] * 3) framePoseEff = pinocchio.SE3.Identity() framePoseEff.translation = target Pref = crocoddyl.FramePlacement(endEffectorId, framePoseEff) goalTrackingCost = crocoddyl.CostModelFramePlacement( state, crocoddyl.ActivationModelWeightedQuad(goaltrackingWeights**2), Pref, actuation.nu)
runningCostModel = crocoddyl.CostModelSum(state, actModel.nu) terminalCostModel = crocoddyl.CostModelSum(state, actModel.nu) # Needed objects to create the costs Mref = crocoddyl.FramePlacement( robot_model.getFrameId("base_link"), pinocchio.SE3(target_quat.matrix(), target_pos)) wBasePos, wBaseOri, wBaseVel = 0.1, 1000, 1000 stateWeights = np.array([wBasePos] * 3 + [wBaseOri] * 3 + [wBaseVel] * robot_model.nv) # Costs goalTrackingCost = crocoddyl.CostModelFramePlacement(state, Mref, actModel.nu) xRegCost = crocoddyl.CostModelState( state, crocoddyl.ActivationModelWeightedQuad(stateWeights), state.zero(), actModel.nu) uRegCost = crocoddyl.CostModelControl(state, actModel.nu) 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, actModel, runningCostModel), dt) terminalModel = crocoddyl.IntegratedActionModelEuler( crocoddyl.DifferentialActionModelFreeFwdDynamics(state, actModel, terminalCostModel), dt) runningModel.u_lb = np.array([l_lim, l_lim, l_lim, l_lim])
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.) limitCost = crocoddyl.CostModelState(state, crocoddyl.ActivationModelQuadraticBarrier(bounds), rmodel.defaultState, actuation.nu) # Cost for state and control stateWeights = np.array([0] * 3 + [10.] * 3 + [0.01] * (state.nv - 6) + [10] * state.nv) stateWeightsTerm = np.array([0] * 3 + [10.] * 3 + [0.01] * (state.nv - 6) + [100] * state.nv) xRegCost = crocoddyl.CostModelState(state, crocoddyl.ActivationModelWeightedQuad(stateWeights**2), rmodel.defaultState, actuation.nu) uRegCost = crocoddyl.CostModelControl(state, actuation.nu) xRegTermCost = crocoddyl.CostModelState(state, crocoddyl.ActivationModelWeightedQuad(stateWeightsTerm**2), rmodel.defaultState, actuation.nu) # Cost for target reaching goaltrackingWeights = np.array([1] * 3 + [0.0001] * 3) framePoseEff = pinocchio.SE3.Identity() framePoseEff.translation = target Pref = crocoddyl.FramePlacement(endEffectorId, framePoseEff) goalTrackingCost = crocoddyl.CostModelFramePlacement(state, crocoddyl.ActivationModelWeightedQuad(goaltrackingWeights**2), Pref, actuation.nu) # Cost for CoM reference
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 T = 100 x0 = np.array([3.14, 0., 0., 0.])
maxfloat * np.matrix(np.ones((6, 1))), # dimension of the SE(3) manifold rmodel.upperPositionLimit[7:], maxfloat * np.matrix(np.ones((state.nv, 1))) ]) bounds = crocoddyl.ActivationBounds(xlb, xub, 1.) limitCost = crocoddyl.CostModelState( state, crocoddyl.ActivationModelQuadraticBarrier(bounds), rmodel.defaultState, actuation.nu) # Cost for state and control stateWeights = np.array([0] * 3 + [10.] * 3 + [0.01] * (state.nv - 6) + [10] * state.nv) stateWeightsTerm = np.array([0] * 3 + [10.] * 3 + [0.01] * (state.nv - 6) + [100] * state.nv) xRegCost = crocoddyl.CostModelState( state, crocoddyl.ActivationModelWeightedQuad(np.matrix(stateWeights**2).T), rmodel.defaultState, actuation.nu) uRegCost = crocoddyl.CostModelControl(state, actuation.nu) xRegTermCost = crocoddyl.CostModelState( state, crocoddyl.ActivationModelWeightedQuad(np.matrix(stateWeightsTerm**2).T), rmodel.defaultState, actuation.nu) # Cost for target reaching goaltrackingWeights = np.array([1] * 3 + [0.0001] * 3) framePoseEff = pinocchio.SE3.Identity() framePoseEff.translation = np.matrix(target).T Pref = crocoddyl.FramePlacement(endEffectorId, framePoseEff) goalTrackingCost = crocoddyl.CostModelFramePlacement( state, crocoddyl.ActivationModelWeightedQuad(np.matrix(goaltrackingWeights**2).T),
tau_f = np.array([[0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0], [1.0, 1.0, 1.0, 1.0], [0.0, d_cog, 0.0, -d_cog], [-d_cog, 0.0, d_cog, 0.0], [-cm / cf, cm / cf, -cm / cf, cm / cf]]) actModel = crocoddyl.ActuationModelMultiCopterBase(state, 4, tau_f) runningCostModel = crocoddyl.CostModelSum(state, actModel.nu) terminalCostModel = crocoddyl.CostModelSum(state, actModel.nu) # Needed objects to create the costs Mref = crocoddyl.FramePlacement(robot_model.getFrameId("base_link"), pinocchio.SE3(target_quat.matrix(), target_pos)) wBasePos, wBaseOri, wBaseVel = 0.1, 1000, 1000 stateWeights = np.array([wBasePos] * 3 + [wBaseOri] * 3 + [wBaseVel] * robot_model.nv) # Costs goalTrackingCost = crocoddyl.CostModelFramePlacement(state, Mref, actModel.nu) xRegCost = crocoddyl.CostModelState(state, crocoddyl.ActivationModelWeightedQuad(stateWeights), state.zero(), actModel.nu) uRegCost = crocoddyl.CostModelControl(state, actModel.nu) 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, actModel, runningCostModel), dt) terminalModel = crocoddyl.IntegratedActionModelEuler( crocoddyl.DifferentialActionModelFreeFwdDynamics(state, actModel, terminalCostModel), dt) # Creating the shooting problem and the FDDP solver T = 33
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) handTrackingActivation = crocoddyl.ActivationModelWeightedQuad(np.array([1] * 3 + [0.0001] * 3)**2) handTrackingCost = crocoddyl.CostModelResidual(state, handTrackingActivation, handTrackingResidual) footTrackingResidual = crocoddyl.ResidualModelFramePlacement(state, leftFootId, pinocchio.SE3(np.eye(3), np.array([0., 0.4, 0.])),
maxfloat * np.matrix(np.ones((6, 1))), # dimension of the SE(3) manifold rmodel.upperPositionLimit[7:], maxfloat * np.matrix(np.ones((state.nv, 1))) ]) bounds = crocoddyl.ActivationBounds(xlb, xub, 1.) limitCost = crocoddyl.CostModelState( state, crocoddyl.ActivationModelQuadraticBarrier(bounds), rmodel.defaultState, actuation.nu) # Cost for state and control stateWeights = np.array([0] * 3 + [10.] * 3 + [0.01] * (state.nv - 6) + [10] * state.nv) stateWeightsTerm = np.array([0] * 3 + [10.] * 3 + [0.01] * (state.nv - 6) + [100] * state.nv) xRegCost = crocoddyl.CostModelState( state, crocoddyl.ActivationModelWeightedQuad(np.matrix(stateWeights**2).T), rmodel.defaultState, actuation.nu) uRegCost = crocoddyl.CostModelControl(state, actuation.nu) xRegTermCost = crocoddyl.CostModelState( state, crocoddyl.ActivationModelWeightedQuad(np.matrix(stateWeightsTerm**2).T), rmodel.defaultState, actuation.nu) # Cost for target reaching: hand and foot handTrackingWeights = np.array([1] * 3 + [0.0001] * 3) Pref = crocoddyl.FramePlacement(endEffectorId, pinocchio.SE3(np.eye(3), np.matrix(target).T)) handTrackingCost = crocoddyl.CostModelFramePlacement( state, crocoddyl.ActivationModelWeightedQuad(np.matrix(handTrackingWeights**2).T),
robot = example_robot_data.loadDoublePendulum() robot_model = robot.model state = crocoddyl.StateMultibody(robot_model) actModel = ActuationModelDoublePendulum(state, actLink=1) weights = np.array([1, 1, 1, 1] + [0.1] * 2) runningCostModel = crocoddyl.CostModelSum(state, actModel.nu) terminalCostModel = crocoddyl.CostModelSum(state, actModel.nu) xRegCost = crocoddyl.CostModelState(state, crocoddyl.ActivationModelQuad(state.ndx), state.zero(), actModel.nu) uRegCost = crocoddyl.CostModelControl(state, crocoddyl.ActivationModelQuad(1), actModel.nu) xPendCost = CostModelDoublePendulum( state, crocoddyl.ActivationModelWeightedQuad(np.matrix(weights).T), actModel.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, actModel, runningCostModel), dt) terminalModel = crocoddyl.IntegratedActionModelEuler( crocoddyl.DifferentialActionModelFreeFwdDynamics(state, actModel, terminalCostModel), dt)
maxfloat * np.ones(6), # dimension of the SE(3) manifold rmodel.upperPositionLimit[7:], maxfloat * np.ones(state.nv) ]) bounds = crocoddyl.ActivationBounds(xlb, xub, 1.) limitCost = crocoddyl.CostModelState( state, crocoddyl.ActivationModelQuadraticBarrier(bounds), rmodel.defaultState, actuation.nu) # Cost for state and control stateWeights = np.array([0] * 3 + [10.] * 3 + [0.01] * (state.nv - 6) + [10] * state.nv) stateWeightsTerm = np.array([0] * 3 + [10.] * 3 + [0.01] * (state.nv - 6) + [100] * state.nv) xRegCost = crocoddyl.CostModelState( state, crocoddyl.ActivationModelWeightedQuad(stateWeights**2), rmodel.defaultState, actuation.nu) uRegCost = crocoddyl.CostModelControl(state, actuation.nu) xRegTermCost = crocoddyl.CostModelState( state, crocoddyl.ActivationModelWeightedQuad(stateWeightsTerm**2), rmodel.defaultState, actuation.nu) # Cost for target reaching: hand and foot handTrackingWeights = np.array([1] * 3 + [0.0001] * 3) Pref = crocoddyl.FramePlacement(endEffectorId, pinocchio.SE3(np.eye(3), target)) handTrackingCost = crocoddyl.CostModelFramePlacement( state, crocoddyl.ActivationModelWeightedQuad(handTrackingWeights**2), Pref, actuation.nu) footTrackingWeights = np.array([1, 1, 0.1] + [1.] * 3)
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) if self.integrator == 'euler': model = crocoddyl.IntegratedActionModelEuler(dmodel, self.control, timeStep) elif self.integrator == 'rk4': model = crocoddyl.IntegratedActionModelRK(dmodel, self.control, crocoddyl.RKType.four, timeStep) elif self.integrator == 'rk3': model = crocoddyl.IntegratedActionModelRK(dmodel, self.control, crocoddyl.RKType.three, timeStep) elif self.integrator == 'rk2': model = crocoddyl.IntegratedActionModelRK(dmodel, self.control, crocoddyl.RKType.two, timeStep) else: model = crocoddyl.IntegratedActionModelEuler(dmodel, self.control, timeStep) return model