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()