#!/usr/bin/env python
import rospy
import pyexotica as exo
from numpy import array
import math
from pyexotica.publish_trajectory import *
import exotica_core_task_maps_py
import exotica_scipy_solver
from time import sleep
import signal

DT = 0.002

def figure_eight(t):
    return array([0.6, -0.1 + math.sin(t * 2.0 * math.pi * 0.5) * 0.1, 0.5 + math.sin(t * math.pi * 0.5) * 0.2, 0, 0, 0])

class Example:

    def __init__(self):

        # Init vars
        self.t = 0.0

        # Setup exotica
        exo.Setup.init_ros()
        signal.signal(signal.SIGINT, sig_int_handler)
        self.problem = exo.Setup.load_problem(
            '{exotica_examples}/resources/configs/example_ik_joint_velocity_limit_constraint.xml')
        self.q = self.problem.get_scene().get_model_state()
        self.solver = exotica_scipy_solver.SciPyEndPoseSolver(self.problem, method="SLSQP")

    def update(self, event):
        self.problem.set_goal('Position', figure_eight(self.t))
        self.problem.start_state = self.q
        self.problem.get_task_maps()['JointVel'].set_previous_joint_state(self.q)
        q = self.solver.solve()[0]
        publish_pose(q, self.problem)
        qdot = (q - self.q) / DT
        print("qdot =", qdot)
        self.t += DT
        self.q = q

# Start publishing ik
print('Publishing IK')
rospy.init_node('example_node')
rospy.Timer(rospy.Duration(DT), Example().update)
rospy.spin()


