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