'groundKnots': 20,
'flyingKnots': 20
}
cameraTF = [2., 2.68, 0.84, 0.2, 0.62, 0.72, 0.22]
# Creating a both solvers
boxfddp = crocoddyl.SolverBoxFDDP(
gait.createJumpingProblem(x0, jumping_gait['jumpHeight'],
jumping_gait['jumpLength'],
jumping_gait['timeStep'],
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(),
{'walking': {'stepLength': 0.6, 'stepHeight': 0.1,
'timeStep': 0.03, 'stepKnots': 25, 'supportKnots': 1}},
{'walking': {'stepLength': 0.6, 'stepHeight': 0.1,
'timeStep': 0.03, 'stepKnots': 25, 'supportKnots': 1}},
{'walking': {'stepLength': 0.6, 'stepHeight': 0.1,
'timeStep': 0.03, 'stepKnots': 25, 'supportKnots': 1}}]
cameraTF = [3., 3.68, 0.84, 0.2, 0.62, 0.72, 0.22]
ddp = [None] * len(GAITPHASES)
for i, phase in enumerate(GAITPHASES):
for key, value in phase.items():
if key == 'walking':
# Creating a walking problem
ddp[i] = crocoddyl.SolverBoxDDP(
gait.createWalkingProblem(x0, value['stepLength'],
value['stepHeight'],
value['timeStep'],
value['stepKnots'],
value['supportKnots']))
# 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)
# Setting up the 3d walking problem
lfFoot, rfFoot, lhFoot, rhFoot = 'LF_FOOT', 'RF_FOOT', 'LH_FOOT', 'RH_FOOT'
gait = SimpleQuadrupedalGaitProblem(robot_model, lfFoot, rfFoot, lhFoot, rhFoot)
# Defining the initial state of the robot
q0 = robot_model.referenceConfigurations['standing'].copy()
v0 = pinocchio.utils.zero(robot_model.nv)
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(),
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)
runningModel.u_lb = np.array([l_lim, l_lim, l_lim, l_lim])
runningModel.u_ub = np.array([u_lim, u_lim, u_lim, u_lim])
# Creating the shooting problem and the boxddp solver
T = 33
problem = crocoddyl.ShootingProblem(np.concatenate([hector.q0, np.zeros(state.nv)]), [runningModel] * T, terminalModel)
boxddp = crocoddyl.SolverBoxDDP(problem)
boxddp.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)
boxddp.setCallbacks([crocoddyl.CallbackLogger(), crocoddyl.CallbackVerbose(), crocoddyl.CallbackDisplay(display)])
elif WITHDISPLAY:
display = crocoddyl.GepettoDisplay(hector, 4, 4, cameraTF)
boxddp.setCallbacks([crocoddyl.CallbackVerbose(), crocoddyl.CallbackDisplay(display)])
elif WITHPLOT:
boxddp.setCallbacks([crocoddyl.CallbackLogger(), crocoddyl.CallbackVerbose()])
else:
boxddp.setCallbacks([crocoddyl.CallbackVerbose()])
# Solving the problem with the boxddp solver
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
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, actuation, runningCostModel), dt)
terminalModel = crocoddyl.IntegratedActionModelEuler(
crocoddyl.DifferentialActionModelFreeFwdDynamics(state, actuation, terminalCostModel), dt)
runningModel.u_lb = np.array([l_lim, l_lim, l_lim, l_lim])
runningModel.u_ub = np.array([u_lim, u_lim, u_lim, u_lim])
# Creating the shooting problem and the BoxDDP solver
T = 33
problem = crocoddyl.ShootingProblem(np.concatenate([hector.q0, np.zeros(state.nv)]), [runningModel] * T, terminalModel)
solver = crocoddyl.SolverBoxDDP(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 BoxDDP solver