def createProblem(gait_phase):
robot_model = example_robot_data.loadHyQ().model
lfFoot, rfFoot, lhFoot, rhFoot = 'lf_foot', 'rf_foot', 'lh_foot', 'rh_foot'
gait = SimpleQuadrupedalGaitProblem(robot_model, lfFoot, rfFoot, lhFoot,
rhFoot)
q0 = robot_model.referenceConfigurations['standing'].copy()
v0 = pinocchio.utils.zero(robot_model.nv)
x0 = np.concatenate([q0, v0])
type_of_gait = list(gait_phase.keys())[0]
value = gait_phase[type_of_gait]
if type_of_gait == 'walking':
# Creating a walking problem
problem = gait.createWalkingProblem(x0, value['stepLength'],
value['stepHeight'],
value['timeStep'],
value['stepKnots'],
value['supportKnots'])
elif type_of_gait == 'trotting':
# Creating a trotting problem
problem = gait.createTrottingProblem(x0, value['stepLength'],
value['stepHeight'],
value['timeStep'],
value['stepKnots'],
value['supportKnots'])
elif type_of_gait == 'pacing':
# Creating a pacing problem
problem = gait.createPacingProblem(x0, value['stepLength'],
value['stepHeight'],
value['timeStep'],
value['stepKnots'],
value['supportKnots'])
elif type_of_gait == 'bounding':
# Creating a bounding problem
problem = gait.createBoundingProblem(x0, value['stepLength'],
value['stepHeight'],
value['timeStep'],
value['stepKnots'],
value['supportKnots'])
elif type_of_gait == 'jumping':
# Creating a jumping problem
problem = gait.createJumpingProblem(x0, value['jumpHeight'],
value['jumpLength'],
value['timeStep'],
value['groundKnots'],
value['flyingKnots'])
xs = [robot_model.defaultState] * (len(problem.runningModels) + 1)
us = [
m.quasiStatic(d, robot_model.defaultState)
for m, d in list(zip(problem.runningModels, problem.runningDatas))
]
return xs, us, problem
anymal = example_robot_data.loadANYmal()
lims = anymal.model.effortLimit
lims *= 0.4 # reduced artificially the torque limits
anymal.model.effortLimit = lims
lims = anymal.model.velocityLimit
lims *= 0.5
anymal.model.velocityLimit = lims
# 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)
# Defining the walking gait parameters
jumping_gait = {
'jumpHeight': 0.15,
'jumpLength': [0.3, 0., 0.],
'timeStep': 1e-2,
'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'],
import pinocchio
from crocoddyl.utils.quadruped import SimpleQuadrupedalGaitProblem, plotSolution
WITHDISPLAY = 'display' in sys.argv or 'CROCODDYL_DISPLAY' in os.environ
WITHPLOT = 'plot' in sys.argv or 'CROCODDYL_PLOT' in os.environ
# Loading the anymal model
anymal = example_robot_data.load('anymal')
robot_model = anymal.model
lims = robot_model.effortLimit
lims *= 0.5 # reduced artificially the torque limits
robot_model.effortLimit = lims
# 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
from crocoddyl.utils.quadruped import SimpleQuadrupedalGaitProblem, plotSolution
WITHDISPLAY = 'display' in sys.argv or 'CROCODDYL_DISPLAY' in os.environ
WITHPLOT = 'plot' in sys.argv or 'CROCODDYL_PLOT' in os.environ
# Loading the anymal model
anymal = example_robot_data.loadANYmal()
# 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)
# Setting up all tasks
GAITPHASES = [{
'walking': {
'stepLength': 0.25,
'stepHeight': 0.15,
'timeStep': 1e-2,
'stepKnots': 25,
'supportKnots': 2
}
}, {
'trotting': {
'stepLength': 0.15,
'stepHeight': 0.1,
'timeStep': 1e-2,
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 runBenchmark(gait_phase):
robot_model = example_robot_data.loadHyQ().model
lfFoot, rfFoot, lhFoot, rhFoot = 'lf_foot', 'rf_foot', 'lh_foot', 'rh_foot'
gait = SimpleQuadrupedalGaitProblem(robot_model, lfFoot, rfFoot, lhFoot,
rhFoot)
q0 = robot_model.referenceConfigurations['half_sitting'].copy()
v0 = pinocchio.utils.zero(robot_model.nv)
x0 = np.concatenate([q0, v0])
type_of_gait = list(gait_phase.keys())[0]
value = gait_phase[type_of_gait]
if type_of_gait == 'walking':
# Creating a walking problem
ddp = crocoddyl.SolverDDP(
gait.createWalkingProblem(x0, value['stepLength'],
value['stepHeight'], value['timeStep'],
value['stepKnots'],
value['supportKnots']))
elif type_of_gait == 'trotting':
# Creating a trotting problem
ddp = crocoddyl.SolverDDP(
gait.createTrottingProblem(x0, value['stepLength'],
value['stepHeight'], value['timeStep'],
value['stepKnots'],
value['supportKnots']))
elif type_of_gait == 'pacing':
# Creating a pacing problem
ddp = crocoddyl.SolverDDP(
gait.createPacingProblem(x0, value['stepLength'],
value['stepHeight'], value['timeStep'],
value['stepKnots'],
value['supportKnots']))
elif type_of_gait == 'bounding':
# Creating a bounding problem
ddp = crocoddyl.SolverDDP(
gait.createBoundingProblem(x0, value['stepLength'],
value['stepHeight'], value['timeStep'],
value['stepKnots'],
value['supportKnots']))
elif type_of_gait == 'jumping':
# Creating a jumping problem
ddp = crocoddyl.SolverDDP(
gait.createJumpingProblem(x0, value['jumpHeight'],
value['timeStep']))
duration = []
xs = [robot_model.defaultState] * len(ddp.models())
us = [
m.quasicStatic(d, robot_model.defaultState)
for m, d in list(zip(ddp.models(), ddp.datas()))[:-1]
]
for i in range(T):
c_start = time.time()
ddp.solve(xs, us, MAXITER, False, 0.1)
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