def motion_optimimization(workspace, costmap=None): print("Checkint Motion Optimization") trajectory = linear_interpolation_trajectory(q_init=-.22 * np.ones(2), q_goal=.3 * np.ones(2), T=22) objective = initialize_objective(trajectory, workspace, costmap) f = TrajectoryOptimizationViewer(objective, draw=False, draw_gradient=True) algorithms.newton_optimize_trajectory(f, trajectory) f.draw(trajectory)
def optimize_path(objective, workspace, path): """ Optimize path using Netwon's method """ obstacle_cost = demonstrations.obsatcle_potential(workspace) objective.objective.set_problem(workspace, path, obstacle_cost) if DRAW_MODE is not None: objective.reset_objective() objective.viewer.save_images = True objective.viewer.workspace_id += 1 objective.viewer.image_id = 0 objective.viewer.draw_ws_obstacles() algorithms.newton_optimize_trajectory(objective, path, verbose=VERBOSE, maxiter=20) return path
def optimize_path(objective, trajectory, workspace): """ Optimize path using Netwon's method """ initialize_objective(workspace, objective.objective) if DRAW_MODE is not None: objective.reset_objective() objective.viewer.save_images = True objective.viewer.workspace_id += 1 objective.viewer.image_id = 0 objective.viewer.draw_ws_obstacles() algorithms.newton_optimize_trajectory(objective, trajectory, verbose=VERBOSE, maxiter=100) return trajectory
def motion_optimimization(): print("Checkint Motion Optimization") T = 20 trajectory = linear_interpolation_trajectory(q_init=-.22 * np.ones(2), q_goal=.3 * np.ones(2), T=T) objective = MotionOptimization2DCostMap( box=EnvBox(origin=np.array([0, 0]), dim=np.array([1., 1.])), T=trajectory.T(), q_init=trajectory.initial_configuration(), q_goal=trajectory.final_configuration()) f = TrajectoryOptimizationViewer(objective, draw=False, draw_gradient=True) for t in range(T + 1): initialize_objective(objective, trajectory) algorithms.newton_optimize_trajectory(objective.objective, trajectory) f.draw(trajectory) resample(trajectory)
def optimize_geodesic(workspace, phi, q_init, q_goal): trajectory = linear_interpolation_trajectory(q_init, q_goal, T=TRAJ_LENGTH) objective = GeodesicObjective2D(T=trajectory.T(), n=trajectory.n(), q_init=trajectory.initial_configuration(), q_goal=trajectory.final_configuration(), embedding=None) sdf = SignedDistanceWorkspaceMap(workspace) cost = CostGridPotential2D(sdf, ALPHA, MARGIN, OFFSET) objective.embedding = phi objective.obstacle_potential = cost objective.workspace = workspace objective.create_clique_network() algorithms.newton_optimize_trajectory(objective.objective, trajectory, verbose=VERBOSE, maxiter=100) return trajectory.list_configurations()
objective_traj.set_scalars(obstacle_scalar=1., init_potential_scalar=0., term_potential_scalar=10000000., acceleration_scalar=2.) objective_traj.create_clique_network() objective_traj.add_final_velocity_terms() objective_traj.add_smoothness_terms(2) objective_traj.add_obstacle_terms() objective_traj.add_box_limits() objective_traj.add_init_and_terminal_terms() objective_traj.create_objective() objective = TrajectoryOptimizationViewer(objective_traj, draw=True, draw_gradient=True) objective.reset_objective() objective.viewer.draw_ws_obstacles() # ---------------------------------------------------------------------------- # Runs a Newton optimization algorithm on the objective # ---------------------------------------------------------------------------- t_0 = time.time() algorithms.newton_optimize_trajectory(objective, trajectory, verbose=True, maxiter=100) print("Done. ({} sec.)".format(time.time() - t_0)) while True: objective.draw(trajectory)
box=EnvBox(), T=T_length, q_init=trajectory.initial_configuration(), q_goal=trajectory.final_configuration()) objective.create_clique_network() objective.add_smoothness_terms(2) objective.add_box_limits() objective.add_init_and_terminal_terms() objective.add_waypoint_terms(q_grasp_approach, t_grasp - 2, 100000.) objective.add_waypoint_terms(q_grasp, t_grasp, 100000.) objective.create_objective() # optimize trajectorty t_start = time.time() algorithms.newton_optimize_trajectory(objective.objective, trajectory, verbose=True) print("optim time : {}".format(time.time() - t_start)) # solve LQR lqr = KinematicTrajectoryFollowingLQR(dt=0.1, trajectory=trajectory) lqr.solve_ricatti(Q_p=10, Q_v=1, R_a=0.1) # drawing viewer = WorkspaceDrawer(Workspace(), wait_for_keyboard=True) # create squared meshgrid x = np.linspace(-.4, -.1, 3) y = np.linspace(-.4, .4, 5) X, Y = np.meshgrid(x, y) start_points = np.vstack([X.ravel(), Y.ravel()]).transpose()