#!/usr/bin/env python

import numpy as np
import pyexotica as exo
from pyexotica.publish_trajectory import *

exo.Setup.init_ros()
solver = exo.Setup.load_solver(
    '{exotica_examples}/resources/configs/example_aico_eff_velocity.xml')
problem = solver.get_problem()

problem.set_rho('Identity', 1e3, 0)
problem.set_goal('Identity', np.array([-1.57, -1.57, 0, 0, 0, 0, 0]), 0)

problem.set_rho('Identity', 1e3, -1)
problem.set_goal('Identity', np.array([1.57, -1.57, 0, 0, 0, 0, 0]), -1)

initial_traj = np.array([-1.57, -1.57, 0, 0, 0, 0, 0]
                        ).reshape(problem.N, 1).repeat(problem.T, 1)

problem.initial_trajectory = initial_traj.T
solution = solver.solve()

plot(solution)

publish_trajectory(solution, problem.T * problem.tau, problem)
