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 __init__(self, state, activation=None, Rref=None, nu=None): activation = activation if activation is not None else crocoddyl.ActivationModelQuad(3) if nu is None: crocoddyl.CostModelAbstract.__init__(self, state, activation) else: crocoddyl.CostModelAbstract.__init__(self, state, activation, nu) self.Rref = Rref
def __init__(self, state, activation, nu): self.nv = state.nv self.nx = state.nx self.nq = state.nq # self.nu = nu if not hasattr(state, 'robot_model'): raise Exception('State needs to have the model parameters, reference the model in state') if not hasattr(state.robot_model, 'T_mu'): # if not specified otherwise, no friction self.T_mu = 0.00 else: self.T_mu = state.robot_model.T_mu if not hasattr(state.robot_model, 'K_m'): # if not specified otherwise, no cost on torque self.K = np.eye(nu) else: self.K = np.array(1/state.robot_model.K_m) self.n = state.robot_model.rotorGearRatio # Modifies the parameters to match the u**2 cost nominal = False if nominal: self.K = np.ones(nu)/2 self.T_mu = np.zeros(nu) self.n = np.ones(nu) self.eps = 1e-3 self.gamma = 1e0 * 1e2/self.n # the partial derivative with respect to the control is stored once, since it is constant self.dTmdu = 1/self.n # next lines are just for the abstraction activation = activation if activation is not None else crocoddyl.ActivationModelQuad(state.ndx) crocoddyl.CostModelAbstract.__init__(self, state, activation, nu = nu)
def __init__(self, state, activation=None, xref=None, nu=None): activation = activation if activation is not None else crocoddyl.ActivationModelQuad(state.ndx) self.xref = xref if xref is not None else state.zero() if nu is None: crocoddyl.CostModelAbstract.__init__(self, state, activation) else: crocoddyl.CostModelAbstract.__init__(self, state, activation, nu)
def __init__(self, state, activation=None, vref=None, nu=None): activation = activation if activation is not None else crocoddyl.ActivationModelQuad( 6) crocoddyl.CostModelAbstract.__init__(self, state, activation, nu) self.vref = vref self.joint = state.pinocchio.frames[vref.frame].parent self.fXj = state.pinocchio.frames[ vref.frame].placement.inverse().action
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 __init__(self, state, activation, nu): if not hasattr(state, 'robot_model'): raise Exception('State needs to have the model parameters, add the model to the state') if not hasattr(state.robot_model, 'T_mu'): state.T_mu = 0.00 if not hasattr(state.robot_model, 'K_m'): state.K_m = 0.00 self.nv = state.nv self.T_mu = state.robot_model.T_mu self.n = state.robot_model.rotorGearRatio activation = activation if activation is not None else crocoddyl.ActivationModelQuad(state.ndx) crocoddyl.CostModelAbstract.__init__(self, state, activation, nu = nu)
def __init__(self, state, activation=None, xref=None, nu=None): activation = activation if activation is not None else crocoddyl.ActivationModelQuad( 3) crocoddyl.CostModelAbstract.__init__(self, state, activation, nu) self.xref = xref
def __init__(self, state, activation=None, uref=None, nu=None): nu = nu if nu is not None else state.nv activation = activation if activation is not None else crocoddyl.ActivationModelQuad( nu) self.uref = uref if uref is not None else pinocchio.utils.zero(nu) crocoddyl.CostModelAbstract.__init__(self, state, activation, nu)
def __init__(self, state, activation, nu): activation = activation if activation is not None else crocoddyl.ActivationModelQuad( state.ndx) crocoddyl.CostModelAbstract.__init__(self, state, activation, nu=nu)
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
WITHDISPLAY = 'display' in sys.argv or 'CROCODDYL_DISPLAY' in os.environ WITHPLOT = 'plot' in sys.argv or 'CROCODDYL_PLOT' in os.environ # 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(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)
WITHDISPLAY = 'display' in sys.argv or 'CROCODDYL_DISPLAY' in os.environ WITHPLOT = 'plot' in sys.argv or 'CROCODDYL_PLOT' in os.environ # 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)
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
runningCostModel = crocoddyl.CostModelSum(state, actuation.nu) terminalCostModel = crocoddyl.CostModelSum(state, actuation.nu) target = np.array(conf.target) footName = 'foot' footFrameID = robot_model.getFrameId(footName) 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))