#!/usr/bin/env python

# Software License Agreement (BSD License)
#
# Copyright (c) 2013, SRI International
# All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
#  * Redistributions of source code must retain the above copyright
#    notice, this list of conditions and the following disclaimer.
#  * Redistributions in binary form must reproduce the above
#    copyright notice, this list of conditions and the following
#    disclaimer in the documentation and/or other materials provided
#    with the distribution.
#  * Neither the name of SRI International nor the names of its
#    contributors may be used to endorse or promote products derived
#    from this software without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
# Author: Acorn Pooley

## BEGIN_SUB_TUTORIAL imports
##
## To use the python interface to move_group, import the moveit_commander
## module.  We also import rospy and some messages that we will use.
import sys
import copy
import math
import rospy
import moveit_commander
import moveit_msgs.msg
import geometry_msgs.msg
import tf
import geometry_msgs.msg
import tf_conversions.posemath as pm
from math import pi
## END_SUB_TUTORIAL

from std_msgs.msg import String

def move_group_python_interface_tutorial():

  print "============ Starting"
  moveit_commander.roscpp_initialize(sys.argv)
  rospy.init_node('goal',
                  anonymous=True)

  robot = moveit_commander.RobotCommander()

  scene = moveit_commander.PlanningSceneInterface()

  group = moveit_commander.MoveGroupCommander("denso_arm")

  display_trajectory_publisher = rospy.Publisher(
                                      '/move_group/display_planned_path',
                                      moveit_msgs.msg.DisplayTrajectory)


  print "============ Planning Frame: %s" % group.get_planning_frame()

  print "============ End effector link: %s" % group.get_end_effector_link()

  print "============ Robot Groups:"
  print robot.get_group_names()
 
#######################################################################

  group.clear_pose_targets()

  #group_variable_values = [0.404 , 1.735 , 0.892 , 0.856 , -0.352 , -0.5]
  #(45)group_variable_values = [0.404 , 1.85 , 0.7 , 0.856 , 0.4 , 2.64]
  group_variable_values = [0.5 , 1.65 , 0.4 , 0 , 1 , 3.65]
  #group_variable_values = [-0.11/180*pi , 90.33/180*pi , 50.94/180*pi , 17.84/180*pi , 32.75/180*pi , 122.25/180*pi]

  group.set_joint_value_target(group_variable_values)
  group.go()
  print "Press Enter"
  raw_input()
  waypoints = []
  wpose = group.get_current_pose().pose

  wpose.position.y += 0.1975
  wpose.orientation.x = 0.5
  waypoints.append(copy.deepcopy(wpose))

  
  wpose.position.x -= 0.02353
  waypoints.append(copy.deepcopy(wpose))

  print waypoints

  (plan, fraction) = group.compute_cartesian_path(
                                    waypoints,
                                    0.01,
                                    0.0)

  group.execute(plan)

  print "============ Finished"


if __name__=='__main__':
  try:
    move_group_python_interface_tutorial()
  except rospy.ROSInterruptException:
    pass