for key, value in phase.items():
if key == 'walking':
# Creating a walking problem
ddp[i] = crocoddyl.SolverDDP(
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)
ddp[i].solve(xs, us, 100, False, 0.1)
# 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)
# Creating the DDP solver for this OC problem, defining a logger
ddp = crocoddyl.SolverDDP(problem)
cameraTF = [2., 2.68, 0.54, 0.2, 0.62, 0.72, 0.22]
if WITHDISPLAY and WITHPLOT:
display = crocoddyl.GepettoDisplay(talos_arm, 4, 4, cameraTF)
ddp.setCallbacks([
crocoddyl.CallbackLogger(),
crocoddyl.CallbackVerbose(),
crocoddyl.CallbackDisplay(display)
])
elif WITHDISPLAY:
display = crocoddyl.GepettoDisplay(talos_arm, 4, 4, cameraTF)
ddp.setCallbacks(
[crocoddyl.CallbackVerbose(),
crocoddyl.CallbackDisplay(display)])
elif WITHPLOT:
ddp.setCallbacks([
crocoddyl.CallbackLogger(),
crocoddyl.CallbackVerbose(),
])
else:
ddp.setCallbacks([crocoddyl.CallbackVerbose()])
# Solving it with the DDP algorithm
runningModel = crocoddyl.IntegratedActionModelEuler(
crocoddyl.DifferentialActionModelFreeFwdDynamics(state, actuation, runningCostModel), dt)
terminalModel = crocoddyl.IntegratedActionModelEuler(
crocoddyl.DifferentialActionModelFreeFwdDynamics(state, actuation, 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)
solver = crocoddyl.SolverFDDP(problem)
solver.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)
solver.setCallbacks([crocoddyl.CallbackLogger(), crocoddyl.CallbackVerbose(), crocoddyl.CallbackDisplay(display)])
elif WITHDISPLAY:
display = crocoddyl.GepettoDisplay(hector, 4, 4, cameraTF)
solver.setCallbacks([crocoddyl.CallbackVerbose(), crocoddyl.CallbackDisplay(display)])
elif WITHPLOT:
solver.setCallbacks([crocoddyl.CallbackLogger(), crocoddyl.CallbackVerbose()])
else:
solver.setCallbacks([crocoddyl.CallbackVerbose()])
# Solving the problem with the FDDP solver
solver.solve()
# Plotting the entire motion
if WITHPLOT:
log = solver.getCallbacks()[0]
crocoddyl.plotOCSolution(log.xs, log.us, figIndex=1, show=False)
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.SolverBoxFDDP(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]))
])
elif WITHPLOT:
solver.setCallbacks(
[crocoddyl.CallbackLogger(),
x0 = np.concatenate([q0, v0])
# Defining the walking gait parameters
walking_gait = {'stepLength': 0.25, 'stepHeight': 0.25, 'timeStep': 1e-2, 'stepKnots': 25, 'supportKnots': 2}
# Setting up the control-limited DDP solver
boxddp = crocoddyl.SolverBoxDDP(
gait.createWalkingProblem(x0, walking_gait['stepLength'], walking_gait['stepHeight'], walking_gait['timeStep'],
walking_gait['stepKnots'], walking_gait['supportKnots']))
# Add the callback functions
print('*** SOLVE ***')
cameraTF = [2., 2.68, 0.84, 0.2, 0.62, 0.72, 0.22]
if WITHDISPLAY and WITHPLOT:
display = crocoddyl.GepettoDisplay(anymal, 4, 4, cameraTF, frameNames=[lfFoot, rfFoot, lhFoot, rhFoot])
boxddp.setCallbacks([crocoddyl.CallbackLogger(), crocoddyl.CallbackVerbose(), crocoddyl.CallbackDisplay(display)])
elif WITHDISPLAY:
display = crocoddyl.GepettoDisplay(anymal, 4, 4, cameraTF, frameNames=[lfFoot, rfFoot, lhFoot, rhFoot])
boxddp.setCallbacks([crocoddyl.CallbackVerbose(), crocoddyl.CallbackDisplay(display)])
elif WITHPLOT:
boxddp.setCallbacks([
crocoddyl.CallbackLogger(),
crocoddyl.CallbackVerbose(),
])
else:
boxddp.setCallbacks([crocoddyl.CallbackVerbose()])
xs = [robot_model.defaultState] * (boxddp.problem.T + 1)
us = boxddp.problem.quasiStatic([anymal.model.defaultState] * boxddp.problem.T)
# Solve the DDP problem
def generateTrajectory(self, **kwargs):
import time
import crocoddyl
from crocoddyl.utils.quadruped import SimpleQuadrupedalGaitProblem, plotSolution
# Load parameters of the trajectory
self.setParametersFromDict(**kwargs)
# Loading the solo model
solo = loadSolo(False)
robot_model = solo.model
# Set limits of actuators speeds and efforts
robot_model.effortLimit[6:] = np.full(12,
self.getParameter('torque_lim'))
robot_model.velocityLimit[6:] = np.tile(
np.deg2rad(self.getParameter('speed_lim')), 4)
# Setting up CoM problem
lfFoot, rfFoot, lhFoot, rhFoot = 'FL_FOOT', 'FR_FOOT', 'HL_FOOT', 'HR_FOOT'
gait = SimpleQuadrupedalGaitProblem(robot_model, lfFoot, rfFoot,
lhFoot, rhFoot)
# Defining the initial state of the robot
q0 = robot_model.referenceConfigurations['standing'].copy()
v0 = pin.utils.zero(robot_model.nv)
x0 = np.concatenate([q0, v0])
# Defining the CoM gait parameters
Jumping_gait = {}
Jumping_gait['jumpHeight'] = self.getParameter('height')
Jumping_gait['jumpLength'] = [
self.getParameter('dx'),
self.getParameter('dy'),
self.getParameter('dz')
]
Jumping_gait['timeStep'] = self.getParameter('dt')
Jumping_gait['groundKnots'] = self.getParameter('groundKnots')
Jumping_gait['flyingKnots'] = self.getParameter('flyingKnots')
# Setting up the control-limited DDP solver
boxddp = crocoddyl.SolverBoxDDP(
gait.createJumpingProblem(x0, Jumping_gait['jumpHeight'],
Jumping_gait['jumpLength'],
Jumping_gait['timeStep'],
Jumping_gait['groundKnots'],
Jumping_gait['flyingKnots']))
# Print debug info if requiered
t0 = time.time()
if self.getParameter('debug'):
print("Computing Trajectory...")
self.printInfo()
# Add the callback functions if requiered
if self.getParameter('verbose'):
boxddp.setCallbacks([crocoddyl.CallbackVerbose()])
# Setup viewer if requiered
cameraTF = [2., 2.68, 0.84, 0.2, 0.62, 0.72, 0.22]
if self.getParameter('gepetto_viewer'):
display = crocoddyl.GepettoDisplay(
solo,
4,
4,
cameraTF,
frameNames=[lfFoot, rfFoot, lhFoot, rhFoot])
boxddp.setCallbacks([
crocoddyl.CallbackVerbose(),
crocoddyl.CallbackDisplay(display)
])
xs = [robot_model.defaultState] * (boxddp.problem.T + 1)
us = boxddp.problem.quasiStatic([solo.model.defaultState] *
boxddp.problem.T)
# Solve the DDP problem
boxddp.solve(xs, us, self.getParameter('nb_it'), False, 0.1)
# Display the entire motion
if self.getParameter('gepetto_viewer'):
display = crocoddyl.GepettoDisplay(
solo, frameNames=[lfFoot, rfFoot, lhFoot, rhFoot])
while True:
display.displayFromSolver(boxddp)
# Get results from solver
xs, us = boxddp.xs, boxddp.us
nx, nq, nu = xs[0].shape[0], robot_model.nq, us[0].shape[0]
X = [0.] * nx
U = [0.] * nu
for i in range(nx):
X[i] = [np.asscalar(x[i]) for x in xs]
for i in range(nu):
U[i] = [np.asscalar(u[i]) if u.shape[0] != 0 else 0 for u in us]
qa = np.array(X[7:nq])[:, :-1]
qa_dot = np.array(X[nq + 6:2 * nq - 1])[:, :-1]
torques = np.array(U[:])
t_traj = np.arange(0, qa.shape[1]) * self.getParameter('dt')
# Print execution time if requiered
if self.getParameter('debug'):
print("Done in", time.time() - t0, "s")
# Define trajectory for return
traj = ActuatorsTrajectory()
traj.addElement('t', t_traj)
traj.addElement('q', qa)
traj.addElement('q_dot', qa_dot)
traj.addElement('torques', torques)
return traj
def run():
# Loading the anymal model
anymal = example_robot_data.load('anymal')
# nq is the dimension fo the configuration vector representation
# nv dimension of the velocity vector space
# Defining the initial state of the robot
q0 = anymal.model.referenceConfigurations['standing'].copy()
v0 = pinocchio.utils.zero(anymal.model.nv)
x0 = np.concatenate([q0, v0])
# Setting up the 3d walking problem
lfFoot, rfFoot, lhFoot, rhFoot = 'LF_FOOT', 'RF_FOOT', 'LH_FOOT', 'RH_FOOT'
gait = SimpleQuadrupedalGaitProblem(anymal.model, lfFoot, rfFoot, lhFoot, rhFoot)
cameraTF = [2., 2.68, 0.84, 0.2, 0.62, 0.72, 0.22]
walking = {
'stepLength': 0.25,
'stepHeight': 0.15,
'timeStep': 1e-2,
'stepKnots': 100,
'supportKnots': 2
}
# Creating a walking problem
ddp = crocoddyl.SolverFDDP(
gait.createWalkingProblem(x0, walking['stepLength'], walking['stepHeight'], walking['timeStep'],
walking['stepKnots'], walking['supportKnots']))
plot = False
display = False
if display:
# Added the callback functions
display = crocoddyl.GepettoDisplay(anymal, 4, 4, cameraTF, frameNames=[lfFoot, rfFoot, lhFoot, rhFoot])
ddp.setCallbacks(
[crocoddyl.CallbackLogger(),
crocoddyl.CallbackVerbose(),
crocoddyl.CallbackDisplay(display)])
# Solving the problem with the DDP solver
xs = [anymal.model.defaultState] * (ddp.problem.T + 1)
us = ddp.problem.quasiStatic([anymal.model.defaultState] * ddp.problem.T)
ddp.solve(xs, us, 100, False, 0.1)
if display:
# Defining the final state as initial one for the next phase
# Display the entire motion
display = crocoddyl.GepettoDisplay(anymal, frameNames=[lfFoot, rfFoot, lhFoot, rhFoot])
display.displayFromSolver(ddp)
# Plotting the entire motion
if plot:
plotSolution(ddp, figIndex=1, show=False)
log = ddp.getCallbacks()[0]
crocoddyl.plotConvergence(log.costs,
log.u_regs,
log.x_regs,
log.grads,
log.stops,
log.steps,
figTitle='walking',
figIndex=3,
show=True)
