def plot_trajectories():
df = pd.read_csv("warmstart.csv")
data = df.values
row = data[0, :]
init_xs = []
init_us = []
starting_state = row[0:3]
init_xs.append(np.array(starting_state))
full_state = np.array(row[3:])
full_state = full_state.reshape(30, 6)
state = full_state[:, 0:3]
control = full_state[:, 3:5]
assert state.shape[0] == control.shape[0]
assert state.shape[1] == 3
assert control.shape[1] == 2
for state in state:
init_xs.append(np.array(state))
for control in control:
init_us.append(np.array(control))
#...... WARMSTARTING CROCODDYL
model = crocoddyl.ActionModelUnicycle()
model.costWeights = np.matrix([1, 0.3]).T
problem = crocoddyl.ShootingProblem(
np.matrix(starting_state).T, [model] * 30, model)
ddp = crocoddyl.SolverDDP(problem)
ddp.solve(init_xs, [], 1000)
ddp_xs = np.array(ddp.xs)
#..... COLDSTARTING CROCODDYL
model2 = crocoddyl.ActionModelUnicycle()
model2.costWeights = np.matrix([1, 0.3]).T
problem2 = crocoddyl.ShootingProblem(
np.matrix(starting_state).T, [model2] * 30, model2)
ddp2 = crocoddyl.SolverDDP(problem2)
ddp2.solve([], [], 1000)
ddp2_xs = np.array(ddp2.xs)
print("PLotting 2 trajectories for comparision \n")
print(
"The value functions, iterations for warmstarted and coldstarted trajectories are as follows ",
"Cost :", ddp.cost, ",", ddp2.cost, "\n", "iterations ", ddp.iter, ",",
ddp2.iter)
fig = plt.figure(figsize=(20, 10))
ax = fig.add_subplot(111)
ax.set_xlim(xmin=-2.1, xmax=2.1)
ax.set_ylim(ymin=-2.1, ymax=2.1)
ax.plot(ddp_xs[:, 0],
ddp_xs[:, 1],
c='red',
label='Warmstarted crocoddyl.')
ax.scatter(ddp2_xs[:, 0],
ddp2_xs[:, 1],
c='green',
label='Coldstarted crocoddyl')
plt.legend()
plt.show()
def warm(self):
net = train_net()
neuralNet = net.generate_trained_net()
"""
state_weight: float = 1., # can use random.uniform(1, 3)
control_weight: float = 0.3, # can use random.uniform(0,1)
nodes: int = 20,
"""
for _ in range(self.__n_trajectories):
initial_config = [
random.uniform(-2.1, 2.),
random.uniform(-2.1, 2.),
random.uniform(0, 1)
]
model = crocoddyl.ActionModelUnicycle()
model2 = crocoddyl.ActionModelUnicycle()
model.costWeights = np.matrix([1., .3]).T
model2.costWeights = np.matrix([1., .3]).T
problem = crocoddyl.ShootingProblem(
np.matrix(initial_config).T, [model] * 20, model)
problem2 = crocoddyl.ShootingProblem(
np.matrix(initial_config).T, [model2] * 20, model2)
ddp = crocoddyl.SolverDDP(problem)
ddp2 = crocoddyl.SolverDDP(problem2)
x_data = np.matrix(initial_config).reshape(1, 3)
prediction = neuralNet.predict(x_data).reshape(20, 5)
xs = np.matrix(prediction[:, 0:3])
us = np.matrix(prediction[:, 3:5])
ddp_xs = []
ddp_xs.append(np.matrix(initial_config).reshape(1, 3).T)
for _ in range(xs.shape[0]):
ddp_xs.append((np.matrix(xs[_]).T))
ddp_us = []
for _ in range(us.shape[0]):
ddp_us.append(np.matrix(us[_]).T)
ddp.solve([], ddp_us)
ddp2.solve()
print(f" With warmstart, without warmstart ")
print(ddp.iter, " ", ddp2.iter)
def solveProblem(self, starting_config):
"""Solve unicycle problems and yield ddp """
model = crocoddyl.ActionModelUnicycle()
if self.terminal_model is not None:
terminal_unicycle = self.terminal_model
else:
terminal_unicycle = crocoddyl.ActionModelUnicycle()
assert terminal_unicycle is not None
model.costWeights = np.array([*self.weights]).T
problem = crocoddyl.ShootingProblem(starting_config, [model] * self.horizon, terminal_unicycle)
ddp = crocoddyl.SolverDDP(problem)
ddp.th_stop = self.precision
ddp.solve([], [], self.maxiters)
del model, terminal_unicycle, problem
return ddp
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
def getTrainingData(ntraj:int = 10000):
trajectory = []
for _ in range(ntraj):
initial_config = [random.uniform(-1.99, 1.99), random.uniform(-1.99, 1.99), 0]
model = crocoddyl.ActionModelUnicycle()
model.costWeights = np.matrix([1, 0.3]).T
problem = crocoddyl.ShootingProblem(np.matrix(initial_config).T, [ model ] * 30, model)
ddp = crocoddyl.SolverFDDP(problem)
ddp.solve([], [], 1000)
if ddp.iter < 1000:
# Store trajectory(ie x, y)
a = np.delete(np.array(ddp.xs), 2, 1)
a = np.array(a.flatten())
b = np.append(a, sum(d.cost for d in ddp.datas()))
trajectory.append(b)
#trajectory.append(sum(d.cost for d in ddp.datas()))
# Dataset 2: Shape (number of init_points, 63)..columns are recurring (x,y).
# The first two columns will form train. The last column is the cost associated with each trajectory
trajectory = np.array(trajectory)
print(trajectory.shape)
df_trajectory = pd.DataFrame(trajectory[0:,0:])
return df_trajectory
def stateData(nTraj: int = 10000,
modelWeight1: float = 1,
modelWeight2: float = 0.4,
timeHorizon=30,
initialTheta: float = 0.,
fddp: bool = False,
varyingInitialTheta: bool = False,
saveData: bool = False):
trajectory = []
model = crocoddyl.ActionModelUnicycle()
for _ in range(nTraj):
if varyingInitialTheta:
initial_config = [
random.uniform(-1.99, 1.99),
random.uniform(-1.99, 1.99),
random.uniform(0, 1)
]
else:
initial_config = [
random.uniform(-1.99, 1.99),
random.uniform(-1.99, 1.99), initialTheta
]
model.costWeights = np.matrix([modelWeight1, modelWeight2]).T
problem = crocoddyl.ShootingProblem(
np.matrix(initial_config).T, [model] * timeHorizon, model)
if fddp:
ddp = crocoddyl.SolverFDDP(problem)
else:
ddp = crocoddyl.SolverDDP(problem)
# Will need to include a assertion check for done in more complicated examples
ddp.solve([], [], 1000)
if ddp.iter < 1000:
# Store trajectory(ie x, y, theta)
a = np.array(ddp.xs)
# trajectory -> (x, y, theta)..(x, y, theta)..
a = np.array(a.flatten())
# append cost at the end of each trajectory
b = np.append(a, sum(d.cost for d in ddp.datas()))
trajectory.append(b)
trajectory_ = np.array(trajectory)
if saveData:
df = pd.DataFrame(trajectory_[0:, 0:])
df.to_csv("trajectories.csv")
print(trajectory_.shape)
print(
"# Dataset 2: Shape (nTraj, 3 X TimeWindow + 1)..columns are recurring ie (x,y, theta),(x,y, theta).... \n\
# The first two columns will form train. The last column is the cost associated with each trajectory"
)
return trajectory_
def solver(starting_condition, T=30, precision=1e-9, maxiters=1000):
"""Solve one crocoddyl problem"""
model = crocoddyl.ActionModelUnicycle()
model.costWeights = np.array([1., 1.]).T
problem = crocoddyl.ShootingProblem(starting_condition, [model] * T, model)
ddp = crocoddyl.SolverDDP(problem)
ddp.th_stop = precision
ddp.solve([], [], maxiters)
def solver_ddp(self, xyz):
"""
Returns the ddp
"""
model = crocoddyl.ActionModelUnicycle()
T = 30
model.costWeights = np.matrix([1, 1]).T
problem = crocoddyl.ShootingProblem(
np.array(xyz).T, [model] * T, model)
ddp = crocoddyl.SolverDDP(problem)
ddp.solve([], [], 1000)
return ddp
def getTrainingData(nTraj: int = 10000,
thetaVal: bool = False,
fddp: bool = False):
model = crocoddyl.ActionModelUnicycle()
trajectory = []
for _ in range(nTraj):
if thetaVal:
initial_config = [
random.uniform(-1.99, 1.99),
random.uniform(-1.99, 1.99),
random.uniform(0., 1.)
]
else:
initial_config = [
random.uniform(-1.99, 1.99),
random.uniform(-1.99, 1.99), 0.0
]
model.costWeights = np.matrix([1, 0.3]).T
problem = crocoddyl.ShootingProblem(
np.matrix(initial_config).T, [model] * 30, model)
if fddp:
ddp = crocoddyl.SolverFDDP(problem)
else:
ddp = crocoddyl.SolverDDP(problem)
ddp.solve([], [], 1000)
if ddp.iter < 1000:
state = []
# Attach x, y, theta. this will form the columns in x_training... index 0, 1, 2
state.extend(i for i in ddp.xs[0])
# Attach control: linear_velocity, angular_velocty.....index 3, 4
state.extend(i for i in ddp.us[0])
# Attach value function: cost....index 5
state.append(sum(d.cost for d in ddp.datas()))
trajectory_control = np.hstack(
(np.array(ddp.xs[1:]), np.array(ddp.us)))
state.extend(i for i in trajectory_control.flatten()
) #..............index 6 to -1.....30 X 5
trajectory.append(state)
data = np.array(trajectory)
df = pd.DataFrame(data)
#....................................
# x_train = data[:,0:3] x,y,z
# y_train = data[:,3:] lv, av, cost, (x, y, theta, control1, control2)
#....................................
return data
def solve_problem(terminal_model=None,
initial_configuration=None,
horizon: int = 100,
precision: float = 1e-9,
maxiters: int = 1000,
state_weight: float = 1.,
control_weight: float = 1.):
"""
Solve the problem for a given initial_position.
@params:
1: terminal_model = Terminal model with neural network inside it, for the crocoddyl problem.
If none, then Crocoddyl Action Model will be used as terminal model.
2: initial_configuration = initial position for the unicycle,
either a list or a numpy array or a tensor.
3: horizon = Time horizon for the unicycle. Defaults to 100
4: stop = ddp.th_stop. Defaults to 1e-9
5: maxiters = maximum iterations allowed for crocoddyl.Defaults to 1000
5: state_weight = defaults to 1.
6: control_weight = defaults to 1.
@returns:
ddp
"""
if isinstance(initial_configuration, list):
initial_configuration = np.array(initial_configuration)
elif isinstance(initial_configuration, torch.Tensor):
initial_configuration = initial_configuration.numpy()
model = crocoddyl.ActionModelUnicycle()
model.costWeights = np.matrix([state_weight, control_weight]).T
if terminal_model is None:
problem = crocoddyl.ShootingProblem(initial_configuration.T,
[model] * horizon, model)
else:
problem = crocoddyl.ShootingProblem(initial_configuration.T,
[model] * horizon, terminal_model)
ddp = crocoddyl.SolverDDP(problem)
ddp.th_stop = precision
ddp.solve([], [], maxiters)
return ddp
def solve_crocoddyl(xyz):
"""
xyz should either be a list or an array
"""
model = crocoddyl.ActionModelUnicycle()
T = 30
model.costWeights = np.matrix([1, 1]).T
problem = crocoddyl.ShootingProblem(m2a(xyz).T, [model] * T, model)
ddp = crocoddyl.SolverDDP(problem)
ddp.solve([], [], 1000)
return ddp
def generate_trajectories_random_xyz(self, save: bool = False):
starting_configurations = []
optimal_trajectories = []
controls = []
for _ in range(self.n_trajectories):
initial_config = [
random.uniform(-2.1, 2.1),
random.uniform(-2.1, 2.1),
random.uniform(0, 1)
]
model = crocoddyl.ActionModelUnicycle()
model.costWeights = np.matrix(
[self.state_weight, self.control_weight]).T
problem = crocoddyl.ShootingProblem(
np.matrix(initial_config).T, [model] * self.knots, model)
ddp = crocoddyl.SolverDDP(problem)
ddp.solve([], [], self.maxiter)
if ddp.isFeasible:
state = np.array(ddp.xs)
control = np.array(ddp.us)
starting_configurations.append(state[0, :])
optimal_trajectories.append(state[1:, :])
controls.append(control)
if save:
f = open(
'./data/variable_initial_state/starting_configurations.pkl',
'wb')
cPickle.dump(starting_configurations,
f,
protocol=cPickle.HIGHEST_PROTOCOL)
g = open("./data/variable_initial_state/optimal_trajectories.pkl",
"wb")
cPickle.dump(optimal_trajectories,
g,
protocol=cPickle.HIGHEST_PROTOCOL)
h = open("./data/variable_initial_state/control.pkl", "wb")
cPickle.dump(controls, h, protocol=cPickle.HIGHEST_PROTOCOL)
f.close(), g.close(), h.close()
else:
return starting_configurations, optimal_trajectories, controls
def _generate_trajectories(self):
"""
This could be done better with pool. But since we are generating a maximum of 10K trajectories,
there' no need for pool
"""
starting_configurations = []
optimal_trajectories = []
feasible_trajectories = 0
for _ in range(self.__n_trajectories):
initial_config = np.matrix([
random.uniform(-2.1, 2.),
random.uniform(-2.1, 2.),
random.uniform(0, 1)
]).T
model = crocoddyl.ActionModelUnicycle()
model.costWeights = np.matrix(
[self.__state_weight, self.__control_weight]).T
problem = crocoddyl.ShootingProblem(initial_config,
[model] * self.__nodes, model)
ddp = crocoddyl.SolverDDP(problem)
ddp.solve()
if ddp.isFeasible:
ddp_xs = np.squeeze(np.asarray(ddp.xs))
ddp_us = np.squeeze(np.asarray(ddp.us))
feasible_trajectories += 1
x = ddp_xs[1:, 0]
y = ddp_xs[1:, 1]
theta = ddp_xs[1:, 2]
velocity = ddp_us[:, 0]
torque = ddp_us[:, 1]
optimal_trajectory = np.hstack((x, y, theta, velocity, torque))
starting_configurations.append(
np.squeeze(np.asarray(initial_config)))
optimal_trajectories.append(optimal_trajectory)
starting_configurations = np.asarray(starting_configurations)
optimal_trajectories = np.asarray(optimal_trajectories)
if self.save_trajectories:
f = open('../../data/x_data.pkl', 'wb')
cPickle.dump(starting_configurations,
f,
protocol=cPickle.HIGHEST_PROTOCOL)
g = open("../../data/y_data.pkl", "wb")
cPickle.dump(optimal_trajectories,
g,
protocol=cPickle.HIGHEST_PROTOCOL)
f.close(), g.close()
return starting_configurations, optimal_trajectories, feasible_trajectories
def generate_data(ntrajectories: int = 100000,
maxiter: int = 100,
knots: int = 20,
state_weight: float = 1., # can use random.uniform(1, 3)
control_weight: float = 0.3, # can use random.uniform(0,1)
):
"""
@ Arguments:
ntrajectories : number of trajectories to generate
maxiter : number of iterations crocoddyl is supposed to make for each trajectory
knots : Knots, e.g T = 20, 30
@ Returns:
initial_states : matrix of starting points, shape (ntrajectories, 3)....x_data
optimal trajectories : matrix of trajectories, shape (ntrajectores, knots).....y_data
Each row of this matrix is:
x1, y1, theta1, v1, w1, x2, y2, theta2, v2, w2, .....so on
"""
starting_configurations = []
optimal_trajectories = []
for _ in range(ntrajectories):
initial_config = [random.uniform(-2.1, 2.),
random.uniform(-2.1, 2.),
random.uniform(0, 1)]
model = crocoddyl.ActionModelUnicycle()
model.costWeights = np.matrix([state_weight, control_weight]).T
problem = crocoddyl.ShootingProblem(np.matrix(initial_config).T, [ model ] * knots, model)
ddp = crocoddyl.SolverDDP(problem)
ddp.solve([], [], maxiter)
if ddp.isFeasible:
state = np.array(ddp.xs)
control = np.array(ddp.us)
optimal_trajectory = np.hstack((state[1:,:], control))
optimal_trajectory = np.ravel(optimal_trajectory)
optimal_trajectories.append(optimal_trajectory)
starting_configurations.append(initial_config)
optimal_trajectories = np.array(optimal_trajectories)
starting_configurations = np.array(starting_configurations)
return starting_configurations, optimal_trajectories
def data(nTraj: int = 1000, fddp: bool = False):
model = crocoddyl.ActionModelUnicycle()
x_data = []
y_data = []
for _ in range(nTraj):
if thetaVal:
initial_config = [
random.uniform(-1.99, 1.99),
random.uniform(-1.99, 1.99),
random.uniform(0., 1.)
]
else:
initial_config = [
random.uniform(-1.99, 1.99),
random.uniform(-1.99, 1.99), 0.0
]
model.costWeights = np.matrix([1, 0.3]).T
problem = crocoddyl.ShootingProblem(
np.matrix(initial_config).T, [model] * 30, model)
if fddp:
ddp = crocoddyl.SolverFDDP(problem)
else:
ddp = crocoddyl.SolverDDP(problem)
ddp.solve([], [], 1000)
if ddp.iter < 1000:
xs = np.array(ddp.xs)
cost = []
for d in ddp.data:
cost.append(d.cost)
cost = np.array(cost).reshape(31, 1)
state = np.hstack((cs, cost))
y_data.append(state.ravel())
x_data.append(initial_config)
x_data = np.array(x_data)
y_data = np.array(y_data)
def testData(ntraj: int = 50):
"""
Data is in the form of (x, y, theta, cost)
"""
model = crocoddyl.ActionModelUnicycle()
cost_trajectory = []
for _ in range(ntraj):
x, y = point(0, 0, 2.1)
initial_config = [x, y, 0]
model.costWeights = np.matrix([1, 0.3]).T
problem = crocoddyl.ShootingProblem(
np.matrix(initial_config).T, [model] * 30, model)
ddp = crocoddyl.SolverFDDP(problem)
ddp.solve([], [], 1000)
if ddp.iter < 1000:
a_ = np.array(ddp.xs)
a = np.array(a_)
b = a.flatten()
c = np.append(a, sum(d.cost for d in ddp.datas()))
cost_trajectory.append(c)
return np.array(cost_trajectory)
class UnicycleFDDPTest(SolverAbstractTestCase):
MODEL = crocoddyl.ActionModelUnicycle()
SOLVER = crocoddyl.SolverFDDP
SOLVER_DER = FDDPDerived
class UnicycleTest(ActionModelAbstractTestCase):
MODEL = crocoddyl.ActionModelUnicycle()
MODEL_DER = UnicycleModelDerived()
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
import crocoddyl
import curves
import eigenpy
import example_robot_data
import hppfcl
import multicontact_api
import numpy as np
import pinocchio
import tsid
with open("/dist") as f:
dist = f.read()
if "20.04" in dist:
print("*" * 74)
print("{: <6s}".format(sys.version.split()[0]))
print(eigenpy.Quaternion(1, 2, 3, 4).norm())
print(hppfcl.Capsule(2, 3).computeVolume())
print(pinocchio.SE3.Identity().inverse())
print(example_robot_data.load("talos").model.nq)
URDF = "/talos_data/robots/talos_left_arm.urdf"
PATH = example_robot_data.robots_loader.getModelPath(URDF)
print(tsid.RobotWrapper(PATH + URDF, [PATH], False).na)
print(crocoddyl.ActionModelUnicycle().nr)
print(
curves.bezier(
np.array([[1, 2, 3], [4, 5, 6], [4, 5, 6], [4, 5, 6], [4, 5, 6]]), 0.2,
1.5).dim())
print(multicontact_api.ContactModel().mu)
def stateData(nTraj:int = 10,
modelWeight1:float = 1,
modelWeight2:float = 0.4,
timeHorizon = 30,
initialTheta:float = 0.,
fddp:bool = False,
varyingInitialTheta:bool = False,
saveData:bool = False
):
data = []
trajectory = []
model = crocoddyl.ActionModelUnicycle()
for _ in range(nTraj):
if varyingInitialTheta:
initial_config = [random.uniform(-1.99, 1.99), random.uniform(-1.99, 1.99), initialTheta]
else:
initial_config = [random.uniform(-1.99, 1.99), random.uniform(-1.99, 1.99), initialTheta]
model.costWeights = np.matrix([modelWeight1, modelWeight2]).T
problem = crocoddyl.ShootingProblem(np.matrix(initial_config).T, [ model ] * timeHorizon, model)
if fddp:
ddp = crocoddyl.SolverFDDP(problem)
else:
ddp = crocoddyl.SolverDDP(problem)
# Will need to include a assertion check for done in more complicated examples
ddp.solve([], [], 1000)
if ddp.iter < 1000:
state = []
# Attach x, y, theta
state.extend(i for i in ddp.xs[0])
# Attach control: linear_velocity, angular_velocty
state.extend(i for i in ddp.us[0])
# Attach value function: cost
state.append(sum(d.cost for d in ddp.datas()))
# Attach the number of iterations
state.append(ddp.iter)
data.append(state)
# Store trajectory(ie x, y)
a = np.delete(np.array(ddp.xs), 2, 1)
a = np.array(a.flatten())
b = np.append(a, sum(d.cost for d in ddp.datas()))
trajectory.append(b)
# Dataset 1: x, y, z, linear_velocity, angular_velocity, value_function, iterations
data = np.array(data)
df = pd.DataFrame(data[0:,0:], columns = ["x_position", "y_position",
"z_position", "linear_velocity",
"angular_velocity", "value_function",
"iterations"])
# Dataset 2: Shape (number of init_points, 63)..columns are recurring (x,y).
# The first two columns will form train.
# The last column is the cost associated with each trajectory
trajectory = np.array(trajectory)
print(trajectory.shape)
print("# Dataset 2: Shape (number of init_points, 63)..columns are recurring ie (x,y),(x,y).... \n\
# The first two columns will form train. The last column is the cost associated with each trajectory")
df_trajectory = pd.DataFrame(trajectory[0:,0:])
if saveData:
df.to_csv("initialStates.csv")
df_trajectory.to_csv("trajectory.csv")
print(f"\nReturning {list(df.columns) } for {nTraj} trajectories.")
print("\nReturing trajectories.")
return df, df_trajectory
def iter_analysis():
iters = []
iters_500 = []
iters_1000 = []
for _ in range(10000):
initial_config = [
random.uniform(-2.1, 2.1),
random.uniform(-2.1, 2.1), 0
]
model = crocoddyl.ActionModelUnicycle()
model.costWeights = np.matrix([4, 0.3]).T
problem = crocoddyl.ShootingProblem(
np.matrix(initial_config).T, [model] * 20, model)
ddp = crocoddyl.SolverDDP(problem)
ddp.solve([], [], 100)
iters.append(ddp.iter)
ddp2 = crocoddyl.SolverDDP(problem)
ddp2.solve([], [], 500)
iters_500.append(ddp2.iter)
ddp3 = crocoddyl.SolverDDP(problem)
ddp3.solve([], [], 1000)
iters_1000.append(ddp3.iter)
del ddp, model, ddp2, ddp3
allowed_iterations_500 = []
for _ in range(10000):
initial_config = [
random.uniform(-2.1, 2.1),
random.uniform(-2.1, 2.1), 0
]
model = crocoddyl.ActionModelUnicycle()
model.costWeights = np.matrix([4, 0.3]).T
problem = crocoddyl.ShootingProblem(
np.matrix(initial_config).T, [model] * 20, model)
ddp = crocoddyl.SolverDDP(problem)
ddp.solve([], [], 500)
allowed_iterations_500.append(ddp.iter)
del model, ddp
allowed_iterations_1000 = []
for _ in range(10000):
initial_config = [
random.uniform(-2.1, 2.1),
random.uniform(-2.1, 2.1), 0
]
model = crocoddyl.ActionModelUnicycle()
model.costWeights = np.matrix([4, 0.3]).T
problem = crocoddyl.ShootingProblem(
np.matrix(initial_config).T, [model] * 20, model)
ddp = crocoddyl.SolverDDP(problem)
ddp.solve([], [], 1000)
allowed_iterations_1000.append(ddp.iter)
del ddp, model
failure = 0
for i in zip(iters, iters_1000, iters_500):
if not (i[0] == i[1] == i[2]): failure += 1
print("First:\n")
print(
f"Variance of ddp.iter. For same initial points, ddp.iter remains largely the same \
i.e. for a particular x, y, theta , ddp.solve([], [], 100/500/1000) gives the same ddp.iter. \nThis is expected, however, \
does not give the same result in a few cases.\n \
In this example it crocoddyl failed in {failure} instances over 10 k points.\n"
)
print(
".......................................................................\n"
)
print("Second:\n")
print("Randomly generate 10K points thrice. ")
print(
f"Allowed iterations : 100 | Minimum: {min(iters)} | Maximum: {max(iters)} | Average: {np.mean(iters)} |"
)
print(
f"Allowed iterations : 500 | Minimum: {min(iters_500)} | Maximum: {max(iters_500)} | Average: {np.mean(iters_500)} |"
)
print(
f"Allowed iterations : 1000 | Minimum: {min(iters_1000)} | Maximum: {max(iters_1000)} | Average: {np.mean(iters_1000)} |"
)
del iters, iters_500, iters_1000, allowed_iterations_500, allowed_iterations_1000
import numpy as np
import crocoddyl
from unicycle_utils import plotUnicycleSolution
# Creating an action model for the unicycle system
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.u_regs,
class UnicycleShootingTest(ShootingProblemTestCase):
MODEL = crocoddyl.ActionModelUnicycle()
MODEL_DER = UnicycleModelDerived()
def generate_trained_net(self):
"""
This could be done better with pool. But since we are generating a maximum of 10K trajectories,
there' no need for pool.
@ Description: generate 10K trajectories, each trajectory with same state and control weight.
"""
starting_configurations = []
optimal_trajectories = []
for _ in range(self.__n_trajectories):
initial_config = np.matrix([
random.uniform(-2.1, 2.),
random.uniform(-2.1, 2.),
random.uniform(0, 1)
]).T
model = crocoddyl.ActionModelUnicycle()
model.costWeights = np.matrix(
[self.__state_weight, self.__control_weight]).T
problem = crocoddyl.ShootingProblem(initial_config,
[model] * self.__nodes, model)
ddp = crocoddyl.SolverDDP(problem)
ddp.solve()
if ddp.isFeasible:
state = np.matrix(ddp.xs)
control = np.matrix(ddp.us)
optimal_trajectory = np.hstack((state[1:, :], control))
optimal_trajectory = np.ravel(optimal_trajectory)
optimal_trajectories.append(optimal_trajectory)
"""
So the data set will have self.__nodes * 5 as number of features
"""
starting_configurations.append(ddp.xs[0])
#print(optimal_trajectories)
optimal_trajectories = np.array(optimal_trajectories)
starting_configurations = np.array(starting_configurations)
print(optimal_trajectories.shape)
print(starting_configurations.shape)
if self.save_trajectories:
f = open('x_data.pkl', 'wb')
cPickle.dump(starting_configurations,
f,
protocol=cPickle.HIGHEST_PROTOCOL)
g = open("y_data.pkl", "wb")
cPickle.dump(optimal_trajectories,
g,
protocol=cPickle.HIGHEST_PROTOCOL)
f.close(), g.close()
x_train = starting_configurations[0:9000, :]
y_train = optimal_trajectories[0:9000, :]
x_test = starting_configurations[9000:, :]
y_test = optimal_trajectories[9000:, :]
print(
f'Train size {x_train.shape}, {y_train.shape}, test size {x_test.shape}, {y_test.shape}'
)
model = Sequential()
model.add(Dense(256, input_dim=(starting_configurations.shape[1])))
model.add(Activation('relu'))
for _ in range(self.__n_hidden):
model.add(
Dense(256,
activation="relu",
kernel_initializer='random_uniform',
kernel_regularizer=regularizers.l2(0.01),
activity_regularizer=regularizers.l1(0.01)))
model.add(Dropout(0.25))
model.add(Dense(optimal_trajectories.shape[1], activation='tanh'))
rms = optimizers.RMSprop(lr=0.001, rho=0.9)
sgd = optimizers.SGD(lr=0.01, decay=1e-6, momentum=0.9, nesterov=True)
if self.optimizer == 'sgd':
model.compile(
loss='mean_squared_error',
optimizer=sgd,
metrics=['mean_squared_error', "mean_absolute_error"])
else:
model.compile(loss='mean_squared_error',
optimizer=rms,
metrics=['mean_squared_error'])
print(f'Training neural net on {self.__n_trajectories}...')
model.fit(x_train, y_train, epochs=200, batch_size=16, verbose=1)
#print(score)
print(self.__nodes)
if self.save_model:
model.save('model.h5')
return model
