#!/usr/bin/env python
# EXOTIca IK calculator for poses for wavedemo

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

class ik_outputter:

    def __init__(self):
        self.ik = exo.Setup.load_solver('{wave_demo}/resources/nextage_ik.xml')
        self.ompl = exo.Setup.load_solver('{wave_demo}/resources/nextage_ompl.xml')
        self.solved = self.ik_solve()

    def ik_solve(self):
        self.ik.get_problem().set_rho('wave1', 1e3)
        self.ik.get_problem().set_rho('wave2', 0)
        self.wave1 = self.ik.solve()[0]

        self.ik.get_problem().set_rho('wave1', 0)
        self.ik.get_problem().set_rho('wave2', 1e3)
        self.wave2 = self.ik.solve()[0]

        print self.wave1
        print self.wave2

def main(args):
    exo.Setup.init_ros()
    print("Solving the IK: ")
    wave = ik_outputter()

if __name__ == '__main__':
    main(sys.argv)