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))
