def _solveProblem(self, initial_config, logger: bool = False):
"""Solve one unicycle problem"""
if not isinstance(initial_config, np.ndarray):
initial_config = np.array(initial_config).reshape(-1, 1)
model = crocoddyl.ActionModelUnicycle()
model.costWeights = np.array([*self.weights]).T
if self.terminal_model is None:
problem = crocoddyl.ShootingProblem(initial_config,
[model] * self.horizon, model)
else:
problem = crocoddyl.ShootingProblem(initial_config,
[model] * self.horizon,
self.terminal_model)
ddp = crocoddyl.SolverDDP(problem)
if logger:
log = crocoddyl.CallbackLogger()
ddp.setCallbacks([log])
ddp.th_stop = self.precision
ddp.solve([], [], self.maxiters)
if logger:
#print("\n Returning logs and ddp")
return ddp, log
else:
return ddp
[0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.]).T
# 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()])
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)
# 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()
crocoddyl.DifferentialActionModelFreeFwdDynamics(state, actuationModel,
terminalCostModel))
# Create the problem
q0 = robot_model.referenceConfigurations["tuck_arm"]
qnew = [*q0[0:7], *[0.0, 0.0, 0.0]]
# calculate angle of the wheel
qw1 = np.arctan2(q0[8], q0[9])
qw2 = np.arctan2(q0[10], q0[11])
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.SolverFDDP(problem)
ddp.setCallbacks([crocoddyl.CallbackLogger(), crocoddyl.CallbackVerbose()])
cameraTF = [3., 2.68, 1.54, 0.2, 0.62, 0.72, 0.22]
if WITHDISPLAY and WITHPLOT:
display = crocoddyl.GepettoDisplay(robot, 4, 4, cameraTF)
ddp.setCallbacks([
crocoddyl.CallbackLogger(),
crocoddyl.CallbackVerbose(),
crocoddyl.CallbackDisplay(display)
])
elif WITHDISPLAY:
display = crocoddyl.GepettoDisplay(robot, 4, 4, cameraTF)
ddp.setCallbacks(
[crocoddyl.CallbackVerbose(),
crocoddyl.CallbackDisplay(display)])
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
terminalCostModel)
runningModel1 = crocoddyl.IntegratedActionModelEuler(dmodelRunning1, DT)
runningModel2 = crocoddyl.IntegratedActionModelEuler(dmodelRunning2, DT)
runningModel3 = crocoddyl.IntegratedActionModelEuler(dmodelRunning3, DT)
terminalModel = crocoddyl.IntegratedActionModelEuler(dmodelTerminal, 0)
# Problem definition
x0 = np.concatenate([q0, pinocchio.utils.zero(state.nv)])
problem = crocoddyl.ShootingProblem(x0, [runningModel1] * T + [runningModel2] * T + [runningModel3] * 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(), crocoddyl.CallbackVerbose()])
else:
solver.setCallbacks([crocoddyl.CallbackVerbose()])
# Solving it with the DDP algorithm
xs = [x0] * (solver.problem.T + 1)
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)
def _train(self, neural_net, terminal_unicycle=None):
"""
Args
.........
1: neural_net : neural network to be trained
2: starting_points : trajectories will be generated from these starting points.
3: terminal_unicycle: terminal model for irepa > 0
Returns
........
1: A trained neural net
"""
############################################################ Training ####################################################################################################
starting_points = Datagen.gridData(n_points = self.n_traj)
x_train, y_train1, y_train2 = Solver(initial_configs=starting_points,
terminal_model=terminal_unicycle,
weights=self.weights,
horizon=self.horizon,
precision=self.precision).solveNodesValuesGrads(as_tensor=True)
grads = y_train2.detach().numpy()
cost = y_train1.detach().numpy()
positions = x_train.detach().numpy()
plt.clf()
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
im2 = ax.scatter3D(grads[:,0], grads[:,1], grads[:,2],c = cost, marker = ".", cmap="viridis")
ax.set_title("Grad input to Neural Net")
ax.set_xlabel("Grad[0]")
ax.set_ylabel("Grad[1]")
ax.set_zlabel("Grad[2]")
ax.grid(False)
ax.grid(False)
fig.colorbar(im2).set_label("cost")
plt.savefig("GradInput.png")
plt.show()
plt.clf()
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
im2 = ax.scatter3D(positions[:,0], positions[:,1], positions[:,2],c = cost, marker = ".", cmap="viridis")
ax.set_title("Xtrain, Ytrain for Neural Net")
ax.set_xlabel("X")
ax.set_ylabel("Y")
ax.set_zlabel("Z")
ax.grid(False)
fig.colorbar(im2).set_label("cost")
plt.savefig("StateSpaceInput.png")
plt.show()
del grads, cost, positions
x_valid, y_valid = x_train[0:20, :], y_train1[0:20,:]
x_train = x_train[20:, :]
y_train1 = y_train1[20:, :]
y_train2 = y_train2[20:, :]
# Convert to torch dataloader
dataset = torch.utils.data.TensorDataset(x_train, y_train1, y_train2)
dataloader = torch.utils.data.DataLoader(dataset, batch_size = self.batch_size, shuffle=True)
criterion1 = torch.nn.MSELoss(reduction='sum')
criterion2 = torch.nn.L1Loss(reduction='sum')
criterion3 = torch.nn.MSELoss(reduction='mean')
criterion4 = torch.nn.L1Loss(reduction='mean')
#optimizer = torch.optim.SGD(neural_net.parameters(), lr = self.lr, momentum=0.9, weight_decay=self.weight_decay, nesterov=True)
#optimizer = torch.optim.Adam(neural_net.parameters(), lr = self.lr, betas=[0.5, 0.9], weight_decay=self.weight_decay,amsgrad=True)
optimizer = torch.optim.ASGD(neural_net.parameters(), lr = self.lr, lambd=0.0001, alpha=0.75, t0=1000000.0, weight_decay=self.weight_decay)
neural_net.train()
for epoch in range(self.epochs):
neural_net.train()
for data, target1, target2 in dataloader:
#data.requires_grad=True
optimizer.zero_grad()
output1 = neural_net(data)
output2 = neural_net.batch_jacobian(data)
loss = criterion2(output1, target1) + 0.001*criterion1(output2, target2)
loss.backward()
optimizer.step()
with torch.no_grad():
y_pred = neural_net(x_valid)
error_mse = criterion3(y_pred, y_valid)
error_mae = criterion4(y_pred, y_valid)
print(f"Epoch {epoch + 1} :: mse = {error_mse}, mae = {error_mae}")
### Let's solve a problem
x0 = np.array([-.5, .5, 0.34]).reshape(-1, 1)
model = crocoddyl.ActionModelUnicycle()
model2 = crocoddyl.ActionModelUnicycle()
model.costWeights = np.array([1., 1.]).T
model2.costWeights = np.array([1., 1.]).T
terminal_= TerminalModelUnicycle(neural_net)
problem1 = crocoddyl.ShootingProblem(x0, [model] * 20, model)
problem2 = crocoddyl.ShootingProblem(x0, [model] * 20, terminal_)
ddp1 = crocoddyl.SolverDDP(problem1)
ddp2 = crocoddyl.SolverDDP(problem2)
log1 = crocoddyl.CallbackLogger()
log2 = crocoddyl.CallbackLogger()
ddp1.setCallbacks([log1])
ddp2.setCallbacks([log2])
ddp1.solve([], [], 1000)
ddp2.solve([], [], 1000)
with torch.no_grad():
predict = neural_net(torch.Tensor(x0.T)).detach().numpy().item()
print("\n ddp_c :", ddp1.cost)
print("\n ddp_t :", ddp2.cost)
print("\n net :", predict)
plt.clf()
plt.plot(log1.stops[1:], '--o', label="C")
plt.plot(log2.stops[1:], '--.', label="T")
plt.xlabel("Iterations")
plt.ylabel("Stopping Criteria")
plt.title(f"ddp.cost: {ddp1.cost}, t_ddp.cost: {ddp2.cost}, net_pred_cost: {predict}")
plt.legend()
plt.savefig("sc.png")
plt.show()
starting_points2 = Datagen.gridData(n_points = self.n_traj, xy_limits=[-1, 1], theta_limits=[-0.5, 0.5])
x_test, y_test1, y_test2 = Solver(initial_configs=starting_points2,
terminal_model=terminal_unicycle,
weights=self.weights,
horizon=self.horizon,
precision=self.precision).solveNodesValuesGrads(as_tensor=True)
with torch.no_grad():
pred_cost = neural_net(x_test).detach().numpy()
grads_pred = neural_net.batch_jacobian(x_test).detach().numpy()
x_test = x_test.detach().numpy()
plt.clf()
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
im1 = ax.scatter3D(x_test[:,0], x_test[:,1], x_test[:,2],c = pred_cost, marker = ".", cmap="viridis")
#plt.axis('off')
ax.set_title("Prediction")
ax.set_xlabel("X")
ax.set_ylabel("Y")
ax.set_zlabel("Z")
ax.grid(False)
fig.colorbar(im1).set_label("cost")
plt.savefig('predicted.png')
plt.show()
plt.clf()
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
im2 = ax.scatter3D(grads_pred[:,0], grads_pred[:,1], grads_pred[:,2],c = pred_cost, marker = ".", cmap="viridis")
ax.set_title("jacobian Net")
ax.set_xlabel("Grad[0]")
ax.set_ylabel("Grad[1]")
ax.set_zlabel("Grad[2]")
ax.grid(False)
ax.grid(False)
fig.colorbar(im2).set_label("cost")
plt.savefig('predicted_grads.png')
plt.show()
del dataset, dataloader, x_valid, y_valid, y_pred, x_train, y_train1, y_train2, x_test, y_test1, y_test2
return neural_net
