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 get_data(ntraj: int = 1): data = [] T = 100 for _ in range(ntraj): x0 = [ random.uniform(0, 1), random.uniform(-3.14, 3.14), random.uniform(0., 0.99), random.uniform(0., .99) ] runningModel, terminalModel = cartpole() problem = crocoddyl.ShootingProblem( np.matrix(x0).T, [runningModel] * T, terminalModel) ddp = crocoddyl.SolverFDDP(problem) ddp.solve() if ddp.iter < 1000: position = [] # Attach state four vectors position.extend(float(i) for i in ddp.xs[0]) # Attach control position.extend(float(i) for i in ddp.us[0]) # Attach cost position.append(sum(d.cost for d in ddp.datas())) # Attach the number of iterations position.append(ddp.iter) data.append(position) data = np.array(data)
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 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 runDDPSolveBenchmark(xs, us, problem): ddp = crocoddyl.SolverFDDP(problem) if CALLBACKS: ddp.setCallbacks([crocoddyl.CallbackVerbose()]) duration = [] 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
def solver(starting_condition, T=30, precision=1e-9, maxiters=1000): """Solve one pendulum problem""" robot = example_robot_data.loadDoublePendulum() robot_model = robot.model state = crocoddyl.StateMultibody(robot_model) actModel = ActuationModelDoublePendulum(state, actLink=1) weights = np.array([1.5, 1.5, 1, 1] + [0.1] * 2) runningCostModel = crocoddyl.CostModelSum(state, actModel.nu) dt = 1e-2 xRegCost = crocoddyl.CostModelState( state, crocoddyl.ActivationModelQuad(state.ndx), state.zero(), actModel.nu) uRegCost = crocoddyl.CostModelControl(state, crocoddyl.ActivationModelQuad(1), actModel.nu) xPendCost = CostModelDoublePendulum( state, crocoddyl.ActivationModelWeightedQuad(weights), actModel.nu) runningCostModel.addCost("uReg", uRegCost, 1e-4 / dt) runningCostModel.addCost("xGoal", xPendCost, 1e-5 / dt) runningModel = crocoddyl.IntegratedActionModelEuler( crocoddyl.DifferentialActionModelFreeFwdDynamics( state, actModel, runningCostModel), dt) terminalCostModel = crocoddyl.CostModelSum(state, actModel.nu) terminalCostModel.addCost("xGoal", xPendCost, 1e4) terminal_model = crocoddyl.IntegratedActionModelEuler( crocoddyl.DifferentialActionModelFreeFwdDynamics( state, actModel, terminalCostModel), dt) problem = crocoddyl.ShootingProblem(starting_condition, [runningModel] * T, terminal_model) fddp = crocoddyl.SolverFDDP(problem) fddp.th_stop = precision fddp.solve([], [], maxiters)
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)
goalTrackingCost = crocoddyl.CostModelResidual(state, goalTrackingResidual) runningCostModel.addCost("xReg", xRegCost, 1e-6) 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()])
def optimalSolution(init_states: Union[list, np.ndarray, torch.Tensor], terminal_model: crocoddyl.ActionModelAbstract = None, horizon: int = 150, precision: float = 1e-9, maxiters: int = 1000, use_fddp: bool = True): """Solve double pendulum problem with the given terminal model for the given position Parameters ---------- init_states : list or array or tensor These are the initial, starting configurations for the double pendulum terminal_model: crocoddyl.ActionModelAbstract The terminal model to be used to solve the problem horizon : int Time horizon for the running model precision : float precision for ddp.th_stop maxiters : int Maximum iterations allowed for the problem use_fddp : boolean Solve using ddp or fddp Returns -------- ddp : crocoddyl.Solverddp the optimal ddp or fddp of the prblem """ if isinstance(init_states, torch.Tensor): init_states = init_states.numpy() init_states = np.atleast_2d(init_states) solutions = [] for init_state in init_states: robot = example_robot_data.loadDoublePendulum() robot_model = robot.model state = crocoddyl.StateMultibody(robot_model) actModel = ActuationModelDoublePendulum(state, actLink=1) weights = np.array([1.5, 1.5, 1, 1] + [0.1] * 2) runningCostModel = crocoddyl.CostModelSum(state, actModel.nu) dt = 1e-2 xRegCost = crocoddyl.CostModelState( state, crocoddyl.ActivationModelQuad(state.ndx), state.zero(), actModel.nu) uRegCost = crocoddyl.CostModelControl( state, crocoddyl.ActivationModelQuad(1), actModel.nu) xPendCost = CostModelDoublePendulum( state, crocoddyl.ActivationModelWeightedQuad(weights), actModel.nu) runningCostModel.addCost("uReg", uRegCost, 1e-4 / dt) runningCostModel.addCost("xGoal", xPendCost, 1e-5 / dt) runningModel = crocoddyl.IntegratedActionModelEuler( crocoddyl.DifferentialActionModelFreeFwdDynamics( state, actModel, runningCostModel), dt) if terminal_model is None: terminalCostModel = crocoddyl.CostModelSum(state, actModel.nu) terminalCostModel.addCost("xGoal", xPendCost, 1e4) terminal_model = crocoddyl.IntegratedActionModelEuler( crocoddyl.DifferentialActionModelFreeFwdDynamics( state, actModel, terminalCostModel), dt) problem = crocoddyl.ShootingProblem(init_state, [runningModel] * horizon, terminal_model) if use_fddp: fddp = crocoddyl.SolverFDDP(problem) else: fddp = crocoddyl.SolverDDP(problem) fddp.th_stop = precision fddp.solve([], [], maxiters) solutions.append(fddp) return solutions
'jumpLength': [0.0, 0.3, 0.], 'timeStep': 1e-2, 'groundKnots': 10, 'flyingKnots': 20 } }] cameraTF = [2., 2.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.SolverFDDP( gait.createWalkingProblem(x0, value['stepLength'], value['stepHeight'], value['timeStep'], value['stepKnots'], value['supportKnots'])) elif key == 'trotting': # Creating a trotting problem ddp[i] = crocoddyl.SolverFDDP( gait.createTrottingProblem(x0, value['stepLength'], value['stepHeight'], value['timeStep'], value['stepKnots'], value['supportKnots'])) elif key == 'pacing': # Creating a pacing problem ddp[i] = crocoddyl.SolverFDDP( gait.createPacingProblem(x0, value['stepLength'], value['stepHeight'],
def solve_problem(terminal_model=None, initial_configuration=None, horizon: int = 100, precision: float = 1e-9, maxiters: int = 1000): """ 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 Integrated 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 @returns: ddp """ if isinstance(initial_configuration, list): initial_configuration = np.array(initial_configuration) elif isinstance(initial_configuration, torch.Tensor): initial_configuration = initial_configuration.numpy() # Loading the double pendulum model robot = example_robot_data.loadDoublePendulum() robot_model = robot.model state = crocoddyl.StateMultibody(robot_model) actModel = ActuationModelDoublePendulum(state, actLink=1) weights = np.array([1, 1, 1, 1] + [0.1] * 2) runningCostModel = crocoddyl.CostModelSum(state, actModel.nu) xRegCost = crocoddyl.CostModelState( state, crocoddyl.ActivationModelQuad(state.ndx), state.zero(), actModel.nu) uRegCost = crocoddyl.CostModelControl(state, crocoddyl.ActivationModelQuad(1), actModel.nu) xPendCost = CostModelDoublePendulum( state, crocoddyl.ActivationModelWeightedQuad(np.matrix(weights).T), actModel.nu) dt = 1e-2 runningCostModel.addCost("uReg", uRegCost, 1e-4 / dt) runningCostModel.addCost("xGoal", xPendCost, 1e-5 / dt) runningModel = crocoddyl.IntegratedActionModelEuler( crocoddyl.DifferentialActionModelFreeFwdDynamics( state, actModel, runningCostModel), dt) if terminal_model is None: terminalCostModel = crocoddyl.CostModelSum(state, actModel.nu) terminalCostModel.addCost("xGoal", xPendCost, 1e4) terminal_model = crocoddyl.IntegratedActionModelEuler( crocoddyl.DifferentialActionModelFreeFwdDynamics( state, actModel, terminalCostModel), dt) # Creating the shooting problem and the FDDP solver problem = crocoddyl.ShootingProblem(initial_configuration.T, [runningModel] * horizon, terminal_model) fddp = crocoddyl.SolverFDDP(problem) fddp.th_stop = precision fddp.solve([], [], maxiters) return fddp
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 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)
terminalModel = IntegratedActionModelLPF( crocoddyl.DifferentialActionModelFreeFwdDynamics(state, actuation, terminalCostModel), 0.) # Setting the nodes of the problem with a sliding variable ratioContactTotal = 0.4 / (conf.dt * T) # expressed as ratio in [s] contactNodes = int(conf.T * ratioContactTotal) flyingNodes = conf.T - contactNodes problem_with_contact = crocoddyl.ShootingProblem( x0, [contactPhase] * contactNodes + [runningModel] * flyingNodes, terminalModel) problem_without_contact = crocoddyl.ShootingProblem(x0, [runningModel] * T, terminalModel) # SOLVE ddp = crocoddyl.SolverFDDP(problem_with_contact) ddp.setCallbacks([ crocoddyl.CallbackLogger(), crocoddyl.CallbackVerbose(), ]) ddp.lpf = True # Additionally also modify ddp.th_stop and ddp.th_grad ddp.th_stop = 1e-6 ddp.solve([], [], maxiter=int(1e3)) ddp.robot_model = robot_model # SHOWING THE RESULTS plotOCSolution(ddp) plotConvergence(ddp)