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 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 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 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 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
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
ex_d = ex_m.createData()
x = ex_m.state.rand()
u = np.zeros(2)
a = ex_m.calc(ex_d, x, u)
print(ex_d.xout)
ex_nd = crocoddyl.DifferentialActionModelNumDiff(ex_m, True)
ex_d = ex_nd.createData()
ex_nd.calc(ex_d, x, u)
print('h')
ex_nd.calcDiff(ex_d, x, u)
print(ex_d.Fx)
timeStep = 5e-2
ex_iam = crocoddyl.IntegratedActionModelEuler(ex_nd, timeStep)
x0 = np.matrix([0., 0, np.pi / 2., 0.]).T
T = 50
problem = crocoddyl.ShootingProblem(x0, [ex_iam] * T, ex_iam)
us = [pinocchio.utils.zero(ex_iam.differential.nu)] * T
xs = problem.rollout(us)
import matplotlib.pyplot as plt
fig = plt.figure()
ax = fig.add_subplot(111,
aspect='equal',
autoscale_on=False,
xlim=(-2, 6),
ylim=(-2, 6))