# 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
ddp = crocoddyl.SolverFDDP(problem_with_contact)
ddp.setCallbacks([
crocoddyl.CallbackLogger(),
crocoddyl.CallbackVerbose(),
])
ddp.lpf = True
# Additionally also modify ddp.th_stop and ddp.th_grad
ddp.th_stop = 1e-6
ddp.solve([], [], maxiter=int(1e3))
ddp.robot_model = robot_model
# SHOWING THE RESULTS
plotOCSolution(ddp)
plotConvergence(ddp)
class extractDDPLPF():
def __init__(self, ddp, nu):
self.xs = np.array(ddp.xs)[:, :-nu]
jumping_gait['groundKnots'],
jumping_gait['flyingKnots']))
boxddp = crocoddyl.SolverBoxDDP(
gait.createJumpingProblem(x0, jumping_gait['jumpHeight'],
jumping_gait['jumpLength'],
jumping_gait['timeStep'],
jumping_gait['groundKnots'],
jumping_gait['flyingKnots']))
# Added the callback functions
if WITHDISPLAY and WITHPLOT:
display = crocoddyl.GepettoDisplay(
anymal, 4, 4, cameraTF, frameNames=[lfFoot, rfFoot, lhFoot, rhFoot])
boxfddp.setCallbacks([
crocoddyl.CallbackLogger(),
crocoddyl.CallbackVerbose(),
crocoddyl.CallbackDisplay(display)
])
boxddp.setCallbacks([
crocoddyl.CallbackLogger(),
crocoddyl.CallbackVerbose(),
crocoddyl.CallbackDisplay(display)
])
elif WITHDISPLAY:
display = crocoddyl.GepettoDisplay(
anymal, 4, 4, cameraTF, frameNames=[lfFoot, rfFoot, lhFoot, rhFoot])
boxfddp.setCallbacks(
[crocoddyl.CallbackVerbose(),
crocoddyl.CallbackDisplay(display)])
boxddp.setCallbacks(
[crocoddyl.CallbackVerbose(),
for i, phase in enumerate(GAITPHASES):
for key, value in phase.items():
if key == 'walking':
# Creating a walking problem
ddp[i] = crocoddyl.SolverBoxFDDP(
gait.createWalkingProblem(x0, value['stepLength'], value['stepHeight'], value['timeStep'],
value['stepKnots'], value['supportKnots']))
ddp[i].th_stop = 1e-7
# Added the callback functions
print('*** SOLVE ' + key + ' ***')
if WITHDISPLAY and WITHPLOT:
display = crocoddyl.GepettoDisplay(talos_legs, 4, 4, cameraTF, frameNames=[rightFoot, leftFoot])
ddp[i].setCallbacks(
[crocoddyl.CallbackLogger(),
crocoddyl.CallbackVerbose(),
crocoddyl.CallbackDisplay(display)])
elif WITHDISPLAY:
display = crocoddyl.GepettoDisplay(talos_legs, 4, 4, cameraTF, frameNames=[rightFoot, leftFoot])
ddp[i].setCallbacks([crocoddyl.CallbackVerbose(), crocoddyl.CallbackDisplay(display)])
elif WITHPLOT:
ddp[i].setCallbacks([
crocoddyl.CallbackLogger(),
crocoddyl.CallbackVerbose(),
])
else:
ddp[i].setCallbacks([crocoddyl.CallbackVerbose()])
# Solving the problem with the DDP solver
xs = [talos_legs.model.defaultState] * (ddp[i].problem.T + 1)
us = ddp[i].problem.quasiStatic([talos_legs.model.defaultState] * ddp[i].problem.T)
value['timeStep'],
value['groundKnots'],
value['flyingKnots']))
# Added the callback functions
print('*** SOLVE ' + key + ' ***')
if WITHDISPLAY and WITHPLOT:
display = crocoddyl.GepettoDisplay(
anymal,
4,
4,
cameraTF,
frameNames=[lfFoot, rfFoot, lhFoot, rhFoot])
ddp[i].setCallbacks([
crocoddyl.CallbackLogger(),
crocoddyl.CallbackVerbose(),
crocoddyl.CallbackDisplay(display)
])
elif WITHDISPLAY:
display = crocoddyl.GepettoDisplay(
anymal,
4,
4,
cameraTF,
frameNames=[lfFoot, rfFoot, lhFoot, rhFoot])
ddp[i].setCallbacks(
[crocoddyl.CallbackVerbose(),
crocoddyl.CallbackDisplay(display)])
elif WITHPLOT:
ddp[i].setCallbacks([
crocoddyl.CallbackLogger(),
runningCostModel.addCost("uReg", uRegCost, 1e-6)
runningCostModel.addCost("trackPose", goalTrackingCost, 1e-2)
terminalCostModel.addCost("goalPose", goalTrackingCost, 100)
dt = 3e-2
runningModel = crocoddyl.IntegratedActionModelEuler(
crocoddyl.DifferentialActionModelFreeFwdDynamics(state, actModel, runningCostModel), dt)
terminalModel = crocoddyl.IntegratedActionModelEuler(
crocoddyl.DifferentialActionModelFreeFwdDynamics(state, actModel, terminalCostModel), dt)
# Creating the shooting problem and the FDDP solver
T = 33
problem = crocoddyl.ShootingProblem(np.concatenate([hector.q0, np.zeros(state.nv)]), [runningModel] * T, terminalModel)
fddp = crocoddyl.SolverFDDP(problem)
fddp.setCallbacks([crocoddyl.CallbackLogger(), crocoddyl.CallbackVerbose()])
cameraTF = [-0.03, 4.4, 2.3, -0.02, 0.56, 0.83, -0.03]
if WITHDISPLAY and WITHPLOT:
display = crocoddyl.GepettoDisplay(hector, 4, 4, cameraTF)
fddp.setCallbacks([crocoddyl.CallbackLogger(), crocoddyl.CallbackVerbose(), crocoddyl.CallbackDisplay(display)])
elif WITHDISPLAY:
display = crocoddyl.GepettoDisplay(hector, 4, 4, cameraTF)
fddp.setCallbacks([crocoddyl.CallbackVerbose(), crocoddyl.CallbackDisplay(display)])
elif WITHPLOT:
fddp.setCallbacks([crocoddyl.CallbackLogger(), crocoddyl.CallbackVerbose()])
else:
fddp.setCallbacks([crocoddyl.CallbackVerbose()])
# Solving the problem with the FDDP solver
fddp.solve()
terminalCostModel.addCost("ctrlReg", uRegCost, 1e-7)
# Create the action model
runningModel = crocoddyl.IntegratedActionModelEuler(
crocoddyl.DifferentialActionModelFreeFwdDynamics(state, runningCostModel), DT)
terminalModel = crocoddyl.IntegratedActionModelEuler(
crocoddyl.DifferentialActionModelFreeFwdDynamics(state, terminalCostModel))
#runningModel.differential.armature = 0.2 * np.matrix(np.ones(state.nv)).T
#terminalModel.differential.armature = 0.2 * np.matrix(np.ones(state.nv)).T
# Create the problem
q0 = np.matrix([2., 1.5, -2., 0., 0., 0., 0.]).T
x0 = np.concatenate([q0, pinocchio.utils.zero(state.nv)])
problem = crocoddyl.ShootingProblem(x0, [runningModel] * T, terminalModel)
# Creating the DDP solver for this OC problem, defining a logger
ddp = crocoddyl.SolverDDP(problem)
ddp.setCallbacks([crocoddyl.CallbackVerbose()])
# Solving it with the DDP algorithm
ddp.solve()
# Visualizing the solution in gepetto-viewer
crocoddyl.displayTrajectory(robot, ddp.xs, runningModel.dt)
robot_data = robot_model.createData()
xT = ddp.xs[-1]
pinocchio.forwardKinematics(robot_model, robot_data, xT[:state.nq])
pinocchio.updateFramePlacements(robot_model, robot_data)
print('Finally reached = ', robot_data.oMf[robot_model.getFrameId("gripper_left_joint")].translation.T)
model = crocoddyl.ActionModelUnicycle()
# Setting up the cost weights
model.r = [
10., # state weight
1. # control weight
]
# Formulating the optimal control problem
T = 20 # number of knots
x0 = np.matrix([-1., -1., 1.]).T #x,y,theta
problem = crocoddyl.ShootingProblem(x0, [model] * T, model)
# Creating the DDP solver for this OC problem, defining a logger
ddp = crocoddyl.SolverDDP(problem)
ddp.setCallbacks([crocoddyl.CallbackLogger(), crocoddyl.CallbackVerbose()])
# Solving it with the DDP algorithm
ddp.solve()
# Plotting the solution, solver convergence and unicycle motion
log = ddp.getCallbacks()[0]
crocoddyl.plotOCSolution(log.xs, log.us, figIndex=1, show=False)
crocoddyl.plotConvergence(log.costs,
log.control_regs,
log.state_regs,
log.gm_stops,
log.th_stops,
log.steps,
figIndex=2,
show=False)
state, actuation, contactModel, runningCostModel)
dmodelTerminal = crocoddyl.DifferentialActionModelContactFwdDynamics(
state, actuation, contactModel, terminalCostModel)
runningModel = crocoddyl.IntegratedActionModelEuler(dmodelRunning, DT)
terminalModel = crocoddyl.IntegratedActionModelEuler(dmodelTerminal, 0)
# Problem definition
x0 = np.concatenate([q0, pinocchio.utils.zero(state.nv)])
problem = crocoddyl.ShootingProblem(x0, [runningModel] * T, terminalModel)
# Creating the DDP solver for this OC problem, defining a logger
solver = crocoddyl.SolverFDDP(problem)
if WITHDISPLAY and WITHPLOT:
solver.setCallbacks([
crocoddyl.CallbackLogger(),
crocoddyl.CallbackVerbose(),
crocoddyl.CallbackDisplay(
crocoddyl.GepettoDisplay(robot,
4,
4,
frameNames=[rightFoot, leftFoot]))
])
elif WITHDISPLAY:
solver.setCallbacks([
crocoddyl.CallbackVerbose(),
crocoddyl.CallbackDisplay(
crocoddyl.GepettoDisplay(robot,
4,
4,
frameNames=[rightFoot, leftFoot]))
])