class FramePlacementCostSumTest(CostModelSumTestCase): ROBOT_MODEL = pinocchio.buildSampleModelHumanoidRandom() ROBOT_STATE = crocoddyl.StateMultibody(ROBOT_MODEL) Mref = crocoddyl.FramePlacement(ROBOT_MODEL.getFrameId('rleg5_joint'), pinocchio.SE3.Random()) COST = crocoddyl.CostModelFramePlacement(ROBOT_STATE, Mref)
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
class FramePlacementCostSumTest(CostModelSumTestCase): ROBOT_MODEL = example_robot_data.load('icub_reduced').model ROBOT_STATE = crocoddyl.StateMultibody(ROBOT_MODEL) Mref = crocoddyl.FramePlacement(ROBOT_MODEL.getFrameId('r_sole'), pinocchio.SE3.Random()) COST = crocoddyl.CostModelFramePlacement(ROBOT_STATE, Mref)
class FramePlacementCostTest(CostModelAbstractTestCase): ROBOT_MODEL = example_robot_data.loadICub().model ROBOT_STATE = crocoddyl.StateMultibody(ROBOT_MODEL) Mref = crocoddyl.FramePlacement(ROBOT_MODEL.getFrameId('r_sole'), pinocchio.SE3.Random()) COST = crocoddyl.CostModelFramePlacement(ROBOT_STATE, Mref) COST_DER = FramePlacementCostModelDerived(ROBOT_STATE, Mref=Mref)
def runBenchmark(model): robot_model = ROBOT.model q0 = np.matrix([0.173046, 1., -0.52366, 0., 0., 0.1, -0.005]).T x0 = np.vstack([q0, np.zeros((robot_model.nv, 1))]) # 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. state = crocoddyl.StateMultibody(robot_model) Mref = crocoddyl.FramePlacement( robot_model.getFrameId("gripper_left_joint"), pinocchio.SE3(np.eye(3), np.matrix([[.0], [.0], [.4]]))) goalTrackingCost = crocoddyl.CostModelFramePlacement(state, Mref) xRegCost = crocoddyl.CostModelState(state) uRegCost = crocoddyl.CostModelControl(state) # Create a cost model per the running and terminal action model. runningCostModel = crocoddyl.CostModelSum(state) terminalCostModel = crocoddyl.CostModelSum(state) # Then let's added the running and terminal cost functions runningCostModel.addCost("gripperPose", goalTrackingCost, 1e-3) runningCostModel.addCost("xReg", xRegCost, 1e-7) runningCostModel.addCost("uReg", uRegCost, 1e-7) 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. runningModel = crocoddyl.IntegratedActionModelEuler( model(state, runningCostModel), 1e-3) runningModel.differential.armature = np.matrix( [0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.]).T terminalModel = crocoddyl.IntegratedActionModelEuler( model(state, terminalCostModel), 1e-3) terminalModel.differential.armature = np.matrix( [0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.]).T # For this optimal control problem, we define 100 knots (or running action # models) plus a terminal knot problem = crocoddyl.ShootingProblem(x0, [runningModel] * N, terminalModel) # Creating the DDP solver for this OC problem, defining a logger ddp = crocoddyl.SolverDDP(problem) duration = [] for i in range(T): c_start = time.time() ddp.solve([], [], MAXITER) c_end = time.time() duration.append(1e3 * (c_end - c_start)) avrg_duration = sum(duration) / len(duration) min_duration = min(duration) max_duration = max(duration) return avrg_duration, min_duration, max_duration
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
class FreeFwdDynamicsTest(ActionModelAbstractTestCase): ROBOT_MODEL = pinocchio.buildSampleModelManipulator() STATE = crocoddyl.StateMultibody(ROBOT_MODEL) COST_SUM = crocoddyl.CostModelSum(STATE, ROBOT_MODEL.nv) COST_SUM.addCost('xReg', crocoddyl.CostModelState(STATE), 1.) COST_SUM.addCost( 'frTrack', crocoddyl.CostModelFramePlacement( STATE, crocoddyl.FramePlacement(ROBOT_MODEL.getFrameId("effector_body"), pinocchio.SE3.Random())), 1.) MODEL = crocoddyl.DifferentialActionModelFreeFwdDynamics(STATE, COST_SUM) MODEL_DER = DifferentialFreeFwdDynamicsDerived(STATE, COST_SUM)
def createProblem(model): robot_model = ROBOT.model q0 = np.matrix([0.173046, 1., -0.52366, 0., 0., 0.1, -0.005]).T x0 = np.vstack([q0, np.zeros((robot_model.nv, 1))]) # 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. state = crocoddyl.StateMultibody(robot_model) Mref = crocoddyl.FramePlacement( robot_model.getFrameId("gripper_left_joint"), pinocchio.SE3(np.eye(3), np.matrix([[.0], [.0], [.4]]))) goalTrackingCost = crocoddyl.CostModelFramePlacement(state, Mref) xRegCost = crocoddyl.CostModelState(state) uRegCost = crocoddyl.CostModelControl(state) # Create a cost model per the running and terminal 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("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. actuation = crocoddyl.ActuationModelFull(state) runningModel = crocoddyl.IntegratedActionModelEuler( model(state, actuation, runningCostModel), 1e-3) runningModel.differential.armature = np.matrix( [0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.]).T terminalModel = crocoddyl.IntegratedActionModelEuler( model(state, actuation, terminalCostModel), 1e-3) terminalModel.differential.armature = np.matrix( [0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.]).T # For this optimal control problem, we define 100 knots (or running action # models) plus a terminal knot problem = crocoddyl.ShootingProblem(x0, [runningModel] * N, terminalModel) xs = [x0] * (len(problem.runningModels) + 1) us = [ m.quasiStatic(d, x0) for m, d in list(zip(problem.runningModels, problem.runningDatas)) ] return xs, us, problem
class TalosArmFreeFwdDynamicsTest(ActionModelAbstractTestCase): ROBOT_MODEL = example_robot_data.loadTalosArm().model STATE = crocoddyl.StateMultibody(ROBOT_MODEL) ACTUATION = crocoddyl.ActuationModelFull(STATE) COST_SUM = crocoddyl.CostModelSum(STATE) COST_SUM.addCost( 'gripperPose', crocoddyl.CostModelFramePlacement( STATE, crocoddyl.FramePlacement(ROBOT_MODEL.getFrameId("gripper_left_joint"), pinocchio.SE3.Random())), 1e-3) COST_SUM.addCost("xReg", crocoddyl.CostModelState(STATE), 1e-7) COST_SUM.addCost("uReg", crocoddyl.CostModelControl(STATE), 1e-7) MODEL = crocoddyl.DifferentialActionModelFreeFwdDynamics(STATE, ACTUATION, COST_SUM) MODEL_DER = DifferentialFreeFwdDynamicsDerived(STATE, ACTUATION, COST_SUM)
class ManipulatorDDPTest(SolverAbstractTestCase): ROBOT_MODEL = pinocchio.buildSampleModelManipulator() STATE = crocoddyl.StateMultibody(ROBOT_MODEL) COST_SUM = crocoddyl.CostModelSum(STATE, ROBOT_MODEL.nv) COST_SUM.addCost('xReg', crocoddyl.CostModelState(STATE), 1e-7) COST_SUM.addCost('uReg', crocoddyl.CostModelControl(STATE), 1e-7) COST_SUM.addCost( 'frTrack', crocoddyl.CostModelFramePlacement( STATE, crocoddyl.FramePlacement(ROBOT_MODEL.getFrameId("effector_body"), pinocchio.SE3.Random())), 1.) DIFF_MODEL = crocoddyl.DifferentialActionModelFreeFwdDynamics(STATE, COST_SUM) MODEL = crocoddyl.IntegratedActionModelEuler(crocoddyl.DifferentialActionModelFreeFwdDynamics(STATE, COST_SUM), 1e-3) SOLVER = crocoddyl.SolverDDP SOLVER_DER = DDPDerived
class TalosArmFDDPTest(SolverAbstractTestCase): 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.CostModelFramePlacement( STATE, crocoddyl.FramePlacement( ROBOT_MODEL.getFrameId("gripper_left_joint"), pinocchio.SE3.Random())), 1e-3) COST_SUM.addCost("xReg", crocoddyl.CostModelState(STATE), 1e-7) COST_SUM.addCost("uReg", crocoddyl.CostModelControl(STATE), 1e-7) DIFF_MODEL = crocoddyl.DifferentialActionModelFreeFwdDynamics( STATE, ACTUATION, COST_SUM) MODEL = crocoddyl.IntegratedActionModelEuler(DIFF_MODEL, 1e-3) SOLVER = crocoddyl.SolverFDDP SOLVER_DER = FDDPDerived
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 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('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: footTrack = crocoddyl.CostModelFramePlacement(self.state, i, self.actuation.nu) costModel.addCost("footTrack_" + str(i), footTrack, 1e8) footVel = crocoddyl.FrameMotion(i.frame, pinocchio.Motion.Zero()) impactFootVelCost = crocoddyl.CostModelFrameVelocity(self.state, footVel, self.actuation.nu) costModel.addCost('impactVel_' + str(i.frame), impactFootVelCost, 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-3) # 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
robot_model = talos_arm.model # 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( robot_model.getFrameId("gripper_left_joint"), pinocchio.SE3(np.eye(3), np.matrix([[.0], [.0], [.4]]))) goalTrackingCost = crocoddyl.CostModelFramePlacement(state, Mref) xRegCost = crocoddyl.CostModelState(state) uRegCost = crocoddyl.CostModelControl(state) # 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(
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) Pref = crocoddyl.FramePlacement( leftFootId, pinocchio.SE3(np.eye(3), np.array([0., 0.4, 0.]))) footTrackingCost1 = crocoddyl.CostModelFramePlacement( state, crocoddyl.ActivationModelWeightedQuad(footTrackingWeights**2), Pref, actuation.nu) Pref = crocoddyl.FramePlacement( leftFootId, pinocchio.SE3(np.eye(3), np.array([0.3, 0.15, 0.35]))) footTrackingCost2 = crocoddyl.CostModelFramePlacement( state, crocoddyl.ActivationModelWeightedQuad(footTrackingWeights**2), Pref, actuation.nu) # Cost for CoM reference
[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 comTrack = crocoddyl.CostModelCoMPosition(state, comRef, actuation.nu) # Create cost model per each action model runningCostModel = crocoddyl.CostModelSum(state, actuation.nu) terminalCostModel = crocoddyl.CostModelSum(state, actuation.nu) # Then let's added the running and terminal cost functions runningCostModel.addCost("gripperPose", goalTrackingCost, 1e2) runningCostModel.addCost("stateReg", xRegCost, 1e-3) runningCostModel.addCost("ctrlReg", uRegCost, 1e-4) runningCostModel.addCost("limitCost", limitCost, 1e3)