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
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 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 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)
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 AnymalFreeFwdDynamicsTest(ActionModelAbstractTestCase): ROBOT_MODEL = example_robot_data.loadANYmal().model STATE = crocoddyl.StateMultibody(ROBOT_MODEL) ACTUATION = crocoddyl.ActuationModelFloatingBase(STATE) COST_SUM = crocoddyl.CostModelSum(STATE, ACTUATION.nu) COST_SUM.addCost("xReg", crocoddyl.CostModelState(STATE, ACTUATION.nu), 1e-7) COST_SUM.addCost("uReg", crocoddyl.CostModelControl(STATE, ACTUATION.nu), 1e-7) MODEL = crocoddyl.DifferentialActionModelFreeFwdDynamics(STATE, ACTUATION, COST_SUM) MODEL_DER = DifferentialFreeFwdDynamicsDerived(STATE, ACTUATION, COST_SUM)
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)
class AnymalIntegratedRK4Test(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) DIFFERENTIAL = crocoddyl.DifferentialActionModelFreeFwdDynamics(STATE, ACTUATION, COST_SUM) MODEL = crocoddyl.IntegratedActionModelRK4(DIFFERENTIAL, 1e-3) MODEL_DER = IntegratedActionModelRK4Derived(DIFFERENTIAL, 1e-3)
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)
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 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
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( crocoddyl.DifferentialActionModelFreeFwdDynamics(state, actuationModel, runningCostModel), dt) runningModel.differential.armature = np.matrix( [0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.]).T terminalModel = crocoddyl.IntegratedActionModelEuler( crocoddyl.DifferentialActionModelFreeFwdDynamics(state, actuationModel, terminalCostModel), 0.) 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 250 knots (or running action # models) plus a terminal knot T = 250 q0 = np.matrix([0.173046, 1., -0.52366, 0., 0., 0.1, -0.005]).T x0 = np.concatenate([q0, pinocchio.utils.zero(robot_model.nv)]) problem = crocoddyl.ShootingProblem(x0, [runningModel] * T, terminalModel)
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, 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. dt = 1e-3 runningModel = crocoddyl.IntegratedActionModelEuler( crocoddyl.DifferentialActionModelFreeFwdDynamics(state, runningCostModel), dt) runningModel.differential.armature = np.matrix( [0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.]).T terminalModel = crocoddyl.IntegratedActionModelEuler( crocoddyl.DifferentialActionModelFreeFwdDynamics(state, terminalCostModel), 0.) 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 250 knots (or running action # models) plus a terminal knot T = 250 q0 = np.matrix([0.173046, 1., -0.52366, 0., 0., 0.1, -0.005]).T x0 = np.concatenate([q0, pinocchio.utils.zero(robot_model.nv)]) problem = crocoddyl.ShootingProblem(x0, [runningModel] * T, terminalModel)
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
contactModel, contactCostModel, 0, # inv_damping True) # bool enable force contactPhase = crocoddyl.IntegratedActionModelEuler(contactDifferentialModel, dt) runningCostModel.addCost("joule_dissipation", joule_dissipation, 5e-3) runningCostModel.addCost('joint_friction', joint_friction, 5e-3) # runningCostModel.addCost("velocityRegularization", v2, 1e0) runningCostModel.addCost("nonPenetration", nonPenetration, 1e6) # runningCostModel.addCost("maxJump", maximizeJump, 1e2) terminalCostModel.addCost("footPose", footTrackingCost, 5e3) # terminalCostModel.addCost("footVelocity", footFinalVelocity, 1e0) runningModel = crocoddyl.IntegratedActionModelEuler( crocoddyl.DifferentialActionModelFreeFwdDynamics(state, actuation, runningCostModel), dt) terminalModel = crocoddyl.IntegratedActionModelEuler(crocoddyl.DifferentialActionModelFreeFwdDynamics(state, actuation, terminalCostModel), 0.) runningModel.u_lb = -robot_model.effortLimit[-actuation.nu:] runningModel.u_ub = robot_model.effortLimit[-actuation.nu:] # Setting the nodes of the problem with a sliding variable ratioContactTotal = 0.4/(conf.dt*T) # expressed as ratio in [s] contactNodes = int(conf.T * ratioContactTotal) flyingNodes = conf.T - contactNodes problem_with_contact = crocoddyl.ShootingProblem(x0, [contactPhase] * contactNodes + [runningModel] * flyingNodes, terminalModel) problem_without_contact = crocoddyl.ShootingProblem(x0, [runningModel] * T, terminalModel) # SOLVE