#!/usr/bin/env python
# -*- coding: utf-8 -*-

import sys
import copy
import rospy
import moveit_commander
import moveit_msgs.msg
import geometry_msgs.msg

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

robot = moveit_commander.RobotCommander()
scene = moveit_commander.PlanningSceneInterface()

group = moveit_commander.MoveGroupCommander("manipulator")


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

 print "============ Waiting for RVIZ..."
 rospy.sleep(10)
 print "============ Starting tutorial "
 print "============ Reference frame: %s" % group.get_planning_frame()
 print "============ Robot Groups:"
 print robot.get_group_names()
 print "============ Printing robot state"
 print robot.get_current_state()
 print "============"

 print "============ Generating plan 1"
 pose_target = geometry_msgs.msg.Pose()
 pose_target.orientation.w = 1.0
 pose_target.position.x = 0.7
 pose_target.position.y = -0.05
 pose_target.position.z = 1.1
 group.set_pose_target(pose_target)

 plan1 = group.plan()

 print "============ Waiting while RVIZ displays plan1..."
 rospy.sleep(5)

 waypoints = []

 # start with the current pose
 waypoints.append(group.get_current_pose().pose)

 # first orient gripper and move forward (+x)
 wpose = geometry_msgs.msg.Pose()
 wpose.orientation.w = 1.0
 wpose.position.x = waypoints[0].position.x + 0.1
 wpose.position.y = waypoints[0].position.y
 wpose.position.z = waypoints[0].position.z
 waypoints.append(copy.deepcopy(wpose))

 # second move down
 wpose.position.z -= 0.10
 waypoints.append(copy.deepcopy(wpose))

 # third move to the side
 wpose.position.y += 0.05
 waypoints.append(copy.deepcopy(wpose))



 (plan3, fraction) = group.compute_cartesian_path(
                              waypoints,   # waypoints to follow
                              0.01,        # eef_step
                              0.0)         # jump_threshold

 print "============ Waiting while RVIZ displays plan3..."
 rospy.sleep(5)



 #node: move_group_cube