#!/usr/bin/env python3

import sys
import copy
import rospy
import moveit_commander
import moveit_msgs.msg
import geometry_msgs.msg
from math import pi
from std_msgs.msg import String
from moveit_commander.conversions import pose_to_list
import tf

import numpy as np


def all_close(goal, actual, tolerance):

    """
    Convenience method for testing if a list of values are within a tolerance of their counterparts in another list
    @param: goal       A list of floats, a Pose or a PoseStamped
    @param: actual     A list of floats, a Pose or a PoseStamped
    @param: tolerance  A float
    @returns: bool
    """
    all_equal = True
    if type(goal) is list:
      for index in range(len(goal)):
        if abs(actual[index] - goal[index]) > tolerance:
          return False

    elif type(goal) is geometry_msgs.msg.PoseStamped:
      return all_close(goal.pose, actual.pose, tolerance)

    elif type(goal) is geometry_msgs.msg.Pose:
      return all_close(pose_to_list(goal), pose_to_list(actual), tolerance)

    return True



class BottleGrab(object):
  def __init__(self):
    super(BottleGrab,self).__init__()
    moveit_commander.roscpp_initialize(sys.argv)
    robot = moveit_commander.RobotCommander()
    scene = moveit_commander.PlanningSceneInterface()
    group_name = "manipulator"
    move_group = moveit_commander.MoveGroupCommander(group_name)
    object_pose_sub = rospy.Subscriber("/cutter_node_tf",geometry_msgs.msg.PoseStamped,self.callback,queue_size=10)
    display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path',
                                                   moveit_msgs.msg.DisplayTrajectory,
                                                   queue_size=20)
    planning_frame = move_group.get_planning_frame()
    print("============ Planning frame: %s" % planning_frame)

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

    group_names = robot.get_group_names()
    print("============ Available Planning Groups:", robot.get_group_names())

    print("============ Printing robot state")
    print(robot.get_current_state())
    print("")

    self.robot = robot
    self.scene = scene
    self.move_group = move_group
    self.display_trajectory_publisher = display_trajectory_publisher
    self.object_pose_sub = object_pose_sub
    self.planning_frame = planning_frame
    self.eef_link = eef_link
    self.group_names = group_names

  def callback(self,data):
    #print(data.pose)
    self.object_pose = data
    #goal_position = self.object_pose
    #goal_position.pose.position.y -= 0.1

    # current_pose = self.move_group.get_current_pose().pose
    # current_position = current_pose.position

    #self.move_group.set_pose_target(goal_position)

    #plan = self.move_group.go(wait=True)
    #self.move_group.execute(plan,wait=True)
    # Calling `stop()` ensures that there is no residual movement
    #self.move_group.stop()
    # It is always good to clear your targets after planning with poses.
    # Note: there is no equivalent function for clear_joint_value_targets()
    #self.move_group.clear_pose_targets()
    #input("xD")
    goal_position = self.object_pose.pose.position
    #goal_position.z -= 0.05
    current_pose = self.move_group.get_current_pose().pose
    print(current_pose)
    current_position = current_pose.position

    no_samples = 5

    x_linspace = np.linspace(current_position.x,goal_position.x,num=no_samples)
    y_linspace = np.linspace(current_position.y,goal_position.y,num=no_samples)
    z_linspace = np.linspace(current_position.z,goal_position.z,num=no_samples)

    waypoints = []
    new_pose = current_pose
    new_pose.orientation.x = self.object_pose.pose.orientation.x
    new_pose.orientation.y = self.object_pose.pose.orientation.y
    new_pose.orientation.z = self.object_pose.pose.orientation.z
    new_pose.orientation.w = self.object_pose.pose.orientation.w

    for i in range(no_samples):
      new_pose.position.x = x_linspace[i]
      new_pose.position.y = y_linspace[i]
      new_pose.position.z = z_linspace[i]
      #print(x_linspace[i],y_linspace[i],z_linspace[i])
      waypoints.append(copy.deepcopy(new_pose))

    (plan, fraction) = self.move_group.compute_cartesian_path(
                                   waypoints,   # waypoints to follow
                                   0.01,        # eef_step
                                   0.0)         # jump_threshold
    self.move_group.execute(plan,wait=True)
    print("clamp damp")
    
    
  
  def grab_bootle(self):
    goal_position = self.object_pose.pose.position

    current_pose = self.move_group.get_current_pose().pose
    current_position = current_pose.position

    no_samples = 5

    x_linspace = np.linspace(current_position.x,goal_position.x,num=no_samples)
    y_linspace = np.linspace(current_position.y,goal_position.y,num=no_samples)
    z_linspace = np.linspace(current_position.z,goal_position.z,num=no_samples)

    waypoints = []
    new_pose = current_pose

    for i in range(no_samples):
      new_pose.position.x = x_linspace[i]
      new_pose.position.y = y_linspace[i]
      new_pose.position.z = z_linspace[i]
      print(x_linspace[i],y_linspace[i],z_linspace[i])
      waypoints.append(copy.deepcopy(new_pose))

    (plan, fraction) = self.move_group.compute_cartesian_path(
                                   waypoints,   # waypoints to follow
                                   0.01,        # eef_step
                                   0.0)         # jump_threshold
    self.move_group.execute(plan,wait=True)
    input("( ͡° ͜ʖ ͡°)")

  def go_to_joint_state(self, angle):
    # Copy class variables to local variables to make the web tutorials more clear.
    # In practice, you should use the class variables directly unless you have a good
    # reason not to.
    move_group = self.move_group

    ## BEGIN_SUB_TUTORIAL plan_to_joint_state
    ##
    ## Planning to a Joint Goal
    ## ^^^^^^^^^^^^^^^^^^^^^^^^
    ## The Panda's zero configuration is at a `singularity <https://www.quora.com/Robotics-What-is-meant-by-kinematic-singularity>`_ so the first
    ## thing we want to do is move it to a slightly better configuration.
    # We can get the joint values from the group and adjust some of the values:
    joint_goal = move_group.get_current_joint_values()
    joint_goal[0] = angle
    # joint_goal[1] = 0
    # joint_goal[2] = 0
    # joint_goal[3] = 0
    # joint_goal[4] = 0
    # joint_goal[5] = 0

    # The go command can be called with joint values, poses, or without any
    # parameters if you have already set the pose or joint target for the group
    move_group.go(joint_goal, wait=True)

    # Calling ``stop()`` ensures that there is no residual movement
    move_group.stop()

    ## END_SUB_TUTORIAL

    # For testing:
    current_joints = move_group.get_current_joint_values()
    return all_close(joint_goal, current_joints, 0.01)

  def degrees_to_radians(self,degrees):
      radians = 2*pi*degrees/360
      return radians

  def go_to_joint_state_ur3e(self, angle_vector):
    # Copy class variables to local variables to make the web tutorials more clear.
    # In practice, you should use the class variables directly unless you have a good
    # reason not to.
    move_group = self.move_group

    ## BEGIN_SUB_TUTORIAL plan_to_joint_state
    ##
    ## Planning to a Joint Goal
    ## ^^^^^^^^^^^^^^^^^^^^^^^^
    ## The Panda's zero configuration is at a `singularity <https://www.quora.com/Robotics-What-is-meant-by-kinematic-singularity>`_ so the first
    ## thing we want to do is move it to a slightly better configuration.
    # We can get the joint values from the group and adjust some of the values:
    joint_goal = move_group.get_current_joint_values()
    joint_goal[0] = angle_vector[0]
    joint_goal[1] = angle_vector[1]
    joint_goal[2] = angle_vector[2]
    joint_goal[3] = angle_vector[3]
    joint_goal[4] = angle_vector[4]
    joint_goal[5] = angle_vector[5]

    # The go command can be called with joint values, poses, or without any
    # parameters if you have already set the pose or joint target for the group
    move_group.go(joint_goal, wait=True)

    # Calling ``stop()`` ensures that there is no residual movement
    move_group.stop()

    ## END_SUB_TUTORIAL

    # For testing:
    current_joints = move_group.get_current_joint_values()
    return all_close(joint_goal, current_joints, 0.01)

  def set_quat_from_axis_angle(self,angle,axis):
    s = sin(angle/2)
    x = axis[0] * s
    y = axis[1] * s
    z = axis[2] * s
    w = cos(angle/2)
    return [x,y,z,w]


  def go_to_pose_goal(self,x,y,z,qx,qy,qz,qw):
    # Copy class variables to local variables to make the web tutorials more clear.
    # In practice, you should use the class variables directly unless you have a good
    # reason not to.
    move_group = self.move_group
    pose_goal = geometry_msgs.msg.Pose()
    
    pose_goal.position.x = x
    pose_goal.position.y = y
    pose_goal.position.z = z

    pose_goal.orientation.x = qx
    pose_goal.orientation.y = qy
    pose_goal.orientation.z = qz
    pose_goal.orientation.w = qw
    
    move_group.set_pose_target(pose_goal)

    ## Now, we call the planner to compute the plan and execute it.
    plan = move_group.go(wait=True)
    # Calling `stop()` ensures that there is no residual movement
    move_group.stop()
    # It is always good to clear your targets after planning with poses.
    # Note: there is no equivalent function for clear_joint_value_targets()
    move_group.clear_pose_targets()

    ## END_SUB_TUTORIAL

    # For testing:
    # Note that since this section of code will not be included in the tutorials
    # we use the class variable rather than the copied state variable
    current_pose = self.move_group.get_current_pose().pose
    return all_close(pose_goal, current_pose, 0.01)


  def plan_cartesian_path(self, scale=1):
    # Copy class variables to local variables to make the web tutorials more clear.
    # In practice, you should use the class variables directly unless you have a good
    # reason not to.
    move_group = self.move_group

    ## BEGIN_SUB_TUTORIAL plan_cartesian_path
    ##
    ## Cartesian Paths
    ## ^^^^^^^^^^^^^^^
    ## You can plan a Cartesian path directly by specifying a list of waypoints
    ## for the end-effector to go through. If executing  interactively in a
    ## Python shell, set scale = 1.0.
    ##
    waypoints = []

    wpose = move_group.get_current_pose().pose
    wpose.position.z -= scale * 0.1  # First move up (z)
    wpose.position.y += scale * 0.2  # and sideways (y)
    waypoints.append(copy.deepcopy(wpose))

    wpose.position.x += scale * 0.1  # Second move forward/backwards in (x)
    waypoints.append(copy.deepcopy(wpose))

    wpose.position.y -= scale * 0.1  # Third move sideways (y)
    waypoints.append(copy.deepcopy(wpose))

    # We want the Cartesian path to be interpolated at a resolution of 1 cm
    # which is why we will specify 0.01 as the eef_step in Cartesian
    # translation.  We will disable the jump threshold by setting it to 0.0,
    # ignoring the check for infeasible jumps in joint space, which is sufficient
    # for this tutorial.
    (plan, fraction) = move_group.compute_cartesian_path(
                                       waypoints,   # waypoints to follow
                                       0.01,        # eef_step
                                       0.0)         # jump_threshold

    # Note: We are just planning, not asking move_group to actually move the robot yet:
    return plan, fraction

    ## END_SUB_TUTORIAL


  def display_trajectory(self, plan):
    # Copy class variables to local variables to make the web tutorials more clear.
    # In practice, you should use the class variables directly unless you have a good
    # reason not to.
    robot = self.robot
    display_trajectory_publisher = self.display_trajectory_publisher

    ## BEGIN_SUB_TUTORIAL display_trajectory
    ##
    ## Displaying a Trajectory
    ## ^^^^^^^^^^^^^^^^^^^^^^^
    ## You can ask RViz to visualize a plan (aka trajectory) for you. But the
    ## group.plan() method does this automatically so this is not that useful
    ## here (it just displays the same trajectory again):
    ##
    ## A `DisplayTrajectory`_ msg has two primary fields, trajectory_start and trajectory.
    ## We populate the trajectory_start with our current robot state to copy over
    ## any AttachedCollisionObjects and add our plan to the trajectory.
    display_trajectory = moveit_msgs.msg.DisplayTrajectory()
    display_trajectory.trajectory_start = robot.get_current_state()
    display_trajectory.trajectory.append(plan)
    # Publish
    display_trajectory_publisher.publish(display_trajectory)

    ## END_SUB_TUTORIAL


  def execute_plan(self, plan):
    # Copy class variables to local variables to make the web tutorials more clear.
    # In practice, you should use the class variables directly unless you have a good
    # reason not to.
    move_group = self.move_group

    ## BEGIN_SUB_TUTORIAL execute_plan
    ##
    ## Executing a Plan
    ## ^^^^^^^^^^^^^^^^
    ## Use execute if you would like the robot to follow
    ## the plan that has already been computed:
    move_group.execute(plan, wait=True)

    ## **Note:** The robot's current joint state must be within some tolerance of the
    ## first waypoint in the `RobotTrajectory`_ or ``execute()`` will fail
    ## END_SUB_TUTORIAL


  def wait_for_state_update(self, box_is_known=False, box_is_attached=False, timeout=4):
    # Copy class variables to local variables to make the web tutorials more clear.
    # In practice, you should use the class variables directly unless you have a good
    # reason not to.
    box_name = self.box_name
    scene = self.scene

    ## BEGIN_SUB_TUTORIAL wait_for_scene_update
    ##
    ## Ensuring Collision Updates Are Received
    ## ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
    ## If the Python node dies before publishing a collision object update message, the message
    ## could get lost and the box will not appear. To ensure that the updates are
    ## made, we wait until we see the changes reflected in the
    ## ``get_attached_objects()`` and ``get_known_object_names()`` lists.
    ## For the purpose of this tutorial, we call this function after adding,
    ## removing, attaching or detaching an object in the planning scene. We then wait
    ## until the updates have been made or ``timeout`` seconds have passed
    start = rospy.get_time()
    seconds = rospy.get_time()
    while (seconds - start < timeout) and not rospy.is_shutdown():
      # Test if the box is in attached objects
      attached_objects = scene.get_attached_objects([box_name])
      is_attached = len(attached_objects.keys()) > 0

      # Test if the box is in the scene.
      # Note that attaching the box will remove it from known_objects
      is_known = box_name in scene.get_known_object_names()

      # Test if we are in the expected state
      if (box_is_attached == is_attached) and (box_is_known == is_known):
        return True

      # Sleep so that we give other threads time on the processor
      rospy.sleep(0.1)
      seconds = rospy.get_time()

    # If we exited the while loop without returning then we timed out
    return False
    ## END_SUB_TUTORIAL


def main():
  try:
    rospy.init_node('bottle_grab', anonymous=True)
    bg = BottleGrab()
    try:
        rospy.loginfo("Putarm node BOTTLE GRAB -> is RUN")
        rospy.spin()
    except KeyboardInterrupt:
        print ("Shutting down ROS node")
    rospy.spin()
  except rospy.ROSInterruptException:
      return
  except KeyboardInterrupt:
      return


if __name__ == '__main__':
  main()

#!/usr/bin/env python3

import sys
import copy
import rospy
import moveit_commander
import moveit_msgs.msg
import geometry_msgs.msg
from math import pi
from std_msgs.msg import String
from moveit_commander.conversions import pose_to_list
import tf

import numpy as np


def all_close(goal, actual, tolerance):

    """
    Convenience method for testing if a list of values are within a tolerance of their counterparts in another list
    @param: goal       A list of floats, a Pose or a PoseStamped
    @param: actual     A list of floats, a Pose or a PoseStamped
    @param: tolerance  A float
    @returns: bool
    """
    all_equal = True
    if type(goal) is list:
      for index in range(len(goal)):
        if abs(actual[index] - goal[index]) > tolerance:
          return False

    elif type(goal) is geometry_msgs.msg.PoseStamped:
      return all_close(goal.pose, actual.pose, tolerance)

    elif type(goal) is geometry_msgs.msg.Pose:
      return all_close(pose_to_list(goal), pose_to_list(actual), tolerance)

    return True



class BottleGrab(object):
  def __init__(self):
    super(BottleGrab,self).__init__()
    moveit_commander.roscpp_initialize(sys.argv)
    robot = moveit_commander.RobotCommander()
    scene = moveit_commander.PlanningSceneInterface()
    group_name = "manipulator"
    move_group = moveit_commander.MoveGroupCommander(group_name)
    object_pose_sub = rospy.Subscriber("/cutter_node_tf",geometry_msgs.msg.PoseStamped,self.callback,queue_size=10)
    display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path',
                                                   moveit_msgs.msg.DisplayTrajectory,
                                                   queue_size=20)
    planning_frame = move_group.get_planning_frame()
    print("============ Planning frame: %s" % planning_frame)

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

    group_names = robot.get_group_names()
    print("============ Available Planning Groups:", robot.get_group_names())

    print("============ Printing robot state")
    print(robot.get_current_state())
    print("")

    self.robot = robot
    self.scene = scene
    self.move_group = move_group
    self.display_trajectory_publisher = display_trajectory_publisher
    self.object_pose_sub = object_pose_sub
    self.planning_frame = planning_frame
    self.eef_link = eef_link
    self.group_names = group_names

  def callback(self,data):
    #print(data.pose)
    self.object_pose = data
    #goal_position = self.object_pose
    #goal_position.pose.position.y -= 0.1

    # current_pose = self.move_group.get_current_pose().pose
    # current_position = current_pose.position

    #self.move_group.set_pose_target(goal_position)

    #plan = self.move_group.go(wait=True)
    #self.move_group.execute(plan,wait=True)
    # Calling `stop()` ensures that there is no residual movement
    #self.move_group.stop()
    # It is always good to clear your targets after planning with poses.
    # Note: there is no equivalent function for clear_joint_value_targets()
    #self.move_group.clear_pose_targets()
    #input("xD")
    goal_position = self.object_pose.pose.position
    #goal_position.z -= 0.05
    current_pose = self.move_group.get_current_pose().pose
    print(current_pose)
    current_position = current_pose.position

    no_samples = 5

    x_linspace = np.linspace(current_position.x,goal_position.x,num=no_samples)
    y_linspace = np.linspace(current_position.y,goal_position.y,num=no_samples)
    z_linspace = np.linspace(current_position.z,goal_position.z,num=no_samples)

    waypoints = []
    new_pose = current_pose
    new_pose.orientation.x = self.object_pose.pose.orientation.x
    new_pose.orientation.y = self.object_pose.pose.orientation.y
    new_pose.orientation.z = self.object_pose.pose.orientation.z
    new_pose.orientation.w = self.object_pose.pose.orientation.w

    for i in range(no_samples):
      new_pose.position.x = x_linspace[i]
      new_pose.position.y = y_linspace[i]
      new_pose.position.z = z_linspace[i]
      #print(x_linspace[i],y_linspace[i],z_linspace[i])
      waypoints.append(copy.deepcopy(new_pose))

    (plan, fraction) = self.move_group.compute_cartesian_path(
                                   waypoints,   # waypoints to follow
                                   0.01,        # eef_step
                                   0.0)         # jump_threshold
    self.move_group.execute(plan,wait=True)
    print("clamp damp")
    
    
  
  def grab_bootle(self):
    goal_position = self.object_pose.pose.position

    current_pose = self.move_group.get_current_pose().pose
    current_position = current_pose.position

    no_samples = 5

    x_linspace = np.linspace(current_position.x,goal_position.x,num=no_samples)
    y_linspace = np.linspace(current_position.y,goal_position.y,num=no_samples)
    z_linspace = np.linspace(current_position.z,goal_position.z,num=no_samples)

    waypoints = []
    new_pose = current_pose

    for i in range(no_samples):
      new_pose.position.x = x_linspace[i]
      new_pose.position.y = y_linspace[i]
      new_pose.position.z = z_linspace[i]
      print(x_linspace[i],y_linspace[i],z_linspace[i])
      waypoints.append(copy.deepcopy(new_pose))

    (plan, fraction) = self.move_group.compute_cartesian_path(
                                   waypoints,   # waypoints to follow
                                   0.01,        # eef_step
                                   0.0)         # jump_threshold
    self.move_group.execute(plan,wait=True)
    input("( ͡° ͜ʖ ͡°)")

  def go_to_joint_state(self, angle):
    # Copy class variables to local variables to make the web tutorials more clear.
    # In practice, you should use the class variables directly unless you have a good
    # reason not to.
    move_group = self.move_group

    ## BEGIN_SUB_TUTORIAL plan_to_joint_state
    ##
    ## Planning to a Joint Goal
    ## ^^^^^^^^^^^^^^^^^^^^^^^^
    ## The Panda's zero configuration is at a `singularity <https://www.quora.com/Robotics-What-is-meant-by-kinematic-singularity>`_ so the first
    ## thing we want to do is move it to a slightly better configuration.
    # We can get the joint values from the group and adjust some of the values:
    joint_goal = move_group.get_current_joint_values()
    joint_goal[0] = angle
    # joint_goal[1] = 0
    # joint_goal[2] = 0
    # joint_goal[3] = 0
    # joint_goal[4] = 0
    # joint_goal[5] = 0

    # The go command can be called with joint values, poses, or without any
    # parameters if you have already set the pose or joint target for the group
    move_group.go(joint_goal, wait=True)

    # Calling ``stop()`` ensures that there is no residual movement
    move_group.stop()

    ## END_SUB_TUTORIAL

    # For testing:
    current_joints = move_group.get_current_joint_values()
    return all_close(joint_goal, current_joints, 0.01)

  def degrees_to_radians(self,degrees):
      radians = 2*pi*degrees/360
      return radians

  def go_to_joint_state_ur3e(self, angle_vector):
    # Copy class variables to local variables to make the web tutorials more clear.
    # In practice, you should use the class variables directly unless you have a good
    # reason not to.
    move_group = self.move_group

    ## BEGIN_SUB_TUTORIAL plan_to_joint_state
    ##
    ## Planning to a Joint Goal
    ## ^^^^^^^^^^^^^^^^^^^^^^^^
    ## The Panda's zero configuration is at a `singularity <https://www.quora.com/Robotics-What-is-meant-by-kinematic-singularity>`_ so the first
    ## thing we want to do is move it to a slightly better configuration.
    # We can get the joint values from the group and adjust some of the values:
    joint_goal = move_group.get_current_joint_values()
    joint_goal[0] = angle_vector[0]
    joint_goal[1] = angle_vector[1]
    joint_goal[2] = angle_vector[2]
    joint_goal[3] = angle_vector[3]
    joint_goal[4] = angle_vector[4]
    joint_goal[5] = angle_vector[5]

    # The go command can be called with joint values, poses, or without any
    # parameters if you have already set the pose or joint target for the group
    move_group.go(joint_goal, wait=True)

    # Calling ``stop()`` ensures that there is no residual movement
    move_group.stop()

    ## END_SUB_TUTORIAL

    # For testing:
    current_joints = move_group.get_current_joint_values()
    return all_close(joint_goal, current_joints, 0.01)

  def set_quat_from_axis_angle(self,angle,axis):
    s = sin(angle/2)
    x = axis[0] * s
    y = axis[1] * s
    z = axis[2] * s
    w = cos(angle/2)
    return [x,y,z,w]


  def go_to_pose_goal(self,x,y,z,qx,qy,qz,qw):
    # Copy class variables to local variables to make the web tutorials more clear.
    # In practice, you should use the class variables directly unless you have a good
    # reason not to.
    move_group = self.move_group
    pose_goal = geometry_msgs.msg.Pose()
    
    pose_goal.position.x = x
    pose_goal.position.y = y
    pose_goal.position.z = z

    pose_goal.orientation.x = qx
    pose_goal.orientation.y = qy
    pose_goal.orientation.z = qz
    pose_goal.orientation.w = qw
    
    move_group.set_pose_target(pose_goal)

    ## Now, we call the planner to compute the plan and execute it.
    plan = move_group.go(wait=True)
    # Calling `stop()` ensures that there is no residual movement
    move_group.stop()
    # It is always good to clear your targets after planning with poses.
    # Note: there is no equivalent function for clear_joint_value_targets()
    move_group.clear_pose_targets()

    ## END_SUB_TUTORIAL

    # For testing:
    # Note that since this section of code will not be included in the tutorials
    # we use the class variable rather than the copied state variable
    current_pose = self.move_group.get_current_pose().pose
    return all_close(pose_goal, current_pose, 0.01)


  def plan_cartesian_path(self, scale=1):
    # Copy class variables to local variables to make the web tutorials more clear.
    # In practice, you should use the class variables directly unless you have a good
    # reason not to.
    move_group = self.move_group

    ## BEGIN_SUB_TUTORIAL plan_cartesian_path
    ##
    ## Cartesian Paths
    ## ^^^^^^^^^^^^^^^
    ## You can plan a Cartesian path directly by specifying a list of waypoints
    ## for the end-effector to go through. If executing  interactively in a
    ## Python shell, set scale = 1.0.
    ##
    waypoints = []

    wpose = move_group.get_current_pose().pose
    wpose.position.z -= scale * 0.1  # First move up (z)
    wpose.position.y += scale * 0.2  # and sideways (y)
    waypoints.append(copy.deepcopy(wpose))

    wpose.position.x += scale * 0.1  # Second move forward/backwards in (x)
    waypoints.append(copy.deepcopy(wpose))

    wpose.position.y -= scale * 0.1  # Third move sideways (y)
    waypoints.append(copy.deepcopy(wpose))

    # We want the Cartesian path to be interpolated at a resolution of 1 cm
    # which is why we will specify 0.01 as the eef_step in Cartesian
    # translation.  We will disable the jump threshold by setting it to 0.0,
    # ignoring the check for infeasible jumps in joint space, which is sufficient
    # for this tutorial.
    (plan, fraction) = move_group.compute_cartesian_path(
                                       waypoints,   # waypoints to follow
                                       0.01,        # eef_step
                                       0.0)         # jump_threshold

    # Note: We are just planning, not asking move_group to actually move the robot yet:
    return plan, fraction

    ## END_SUB_TUTORIAL


  def display_trajectory(self, plan):
    # Copy class variables to local variables to make the web tutorials more clear.
    # In practice, you should use the class variables directly unless you have a good
    # reason not to.
    robot = self.robot
    display_trajectory_publisher = self.display_trajectory_publisher

    ## BEGIN_SUB_TUTORIAL display_trajectory
    ##
    ## Displaying a Trajectory
    ## ^^^^^^^^^^^^^^^^^^^^^^^
    ## You can ask RViz to visualize a plan (aka trajectory) for you. But the
    ## group.plan() method does this automatically so this is not that useful
    ## here (it just displays the same trajectory again):
    ##
    ## A `DisplayTrajectory`_ msg has two primary fields, trajectory_start and trajectory.
    ## We populate the trajectory_start with our current robot state to copy over
    ## any AttachedCollisionObjects and add our plan to the trajectory.
    display_trajectory = moveit_msgs.msg.DisplayTrajectory()
    display_trajectory.trajectory_start = robot.get_current_state()
    display_trajectory.trajectory.append(plan)
    # Publish
    display_trajectory_publisher.publish(display_trajectory)

    ## END_SUB_TUTORIAL


  def execute_plan(self, plan):
    # Copy class variables to local variables to make the web tutorials more clear.
    # In practice, you should use the class variables directly unless you have a good
    # reason not to.
    move_group = self.move_group

    ## BEGIN_SUB_TUTORIAL execute_plan
    ##
    ## Executing a Plan
    ## ^^^^^^^^^^^^^^^^
    ## Use execute if you would like the robot to follow
    ## the plan that has already been computed:
    move_group.execute(plan, wait=True)

    ## **Note:** The robot's current joint state must be within some tolerance of the
    ## first waypoint in the `RobotTrajectory`_ or ``execute()`` will fail
    ## END_SUB_TUTORIAL


  def wait_for_state_update(self, box_is_known=False, box_is_attached=False, timeout=4):
    # Copy class variables to local variables to make the web tutorials more clear.
    # In practice, you should use the class variables directly unless you have a good
    # reason not to.
    box_name = self.box_name
    scene = self.scene

    ## BEGIN_SUB_TUTORIAL wait_for_scene_update
    ##
    ## Ensuring Collision Updates Are Received
    ## ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
    ## If the Python node dies before publishing a collision object update message, the message
    ## could get lost and the box will not appear. To ensure that the updates are
    ## made, we wait until we see the changes reflected in the
    ## ``get_attached_objects()`` and ``get_known_object_names()`` lists.
    ## For the purpose of this tutorial, we call this function after adding,
    ## removing, attaching or detaching an object in the planning scene. We then wait
    ## until the updates have been made or ``timeout`` seconds have passed
    start = rospy.get_time()
    seconds = rospy.get_time()
    while (seconds - start < timeout) and not rospy.is_shutdown():
      # Test if the box is in attached objects
      attached_objects = scene.get_attached_objects([box_name])
      is_attached = len(attached_objects.keys()) > 0

      # Test if the box is in the scene.
      # Note that attaching the box will remove it from known_objects
      is_known = box_name in scene.get_known_object_names()

      # Test if we are in the expected state
      if (box_is_attached == is_attached) and (box_is_known == is_known):
        return True

      # Sleep so that we give other threads time on the processor
      rospy.sleep(0.1)
      seconds = rospy.get_time()

    # If we exited the while loop without returning then we timed out
    return False
    ## END_SUB_TUTORIAL


def main():
  try:
    rospy.init_node('bottle_grab', anonymous=True)
    bg = BottleGrab()
    try:
        rospy.loginfo("Putarm node BOTTLE GRAB -> is RUN")
        rospy.spin()
    except KeyboardInterrupt:
        print ("Shutting down ROS node")
    rospy.spin()
  except rospy.ROSInterruptException:
      return
  except KeyboardInterrupt:
      return


if __name__ == '__main__':
  main()

#!/usr/bin/env python3 Convenience method for testing if a list of values are within a tolerance of their counterparts in another list     @param: goal       A list of floats, a Pose or a PoseStamped     @param: actual     A list of floats, a Pose or a PoseStamped     @param: tolerance  A float     @returns: bool #print(data.pose) #goal_position = self.object_pose #goal_position.pose.position.y -= 0.1 # current_pose = self.move_group.get_current_pose().pose # current_position = current_pose.position #self.move_group.set_pose_target(goal_position) #plan = self.move_group.go(wait=True) #self.move_group.execute(plan,wait=True) # Calling `stop()` ensures that there is no residual movement #self.move_group.stop() # It is always good to clear your targets after planning with poses. # Note: there is no equivalent function for clear_joint_value_targets() #self.move_group.clear_pose_targets() #input("xD") #goal_position.z -= 0.05 #print(x_linspace[i],y_linspace[i],z_linspace[i]) # waypoints to follow # eef_step # jump_threshold # waypoints to follow # eef_step # jump_threshold # Copy class variables to local variables to make the web tutorials more clear. # In practice, you should use the class variables directly unless you have a good # reason not to. ## BEGIN_SUB_TUTORIAL plan_to_joint_state ## ## Planning to a Joint Goal ## ^^^^^^^^^^^^^^^^^^^^^^^^ ## The Panda's zero configuration is at a `singularity <https://www.quora.com/Robotics-What-is-meant-by-kinematic-singularity>`_ so the first ## thing we want to do is move it to a slightly better configuration. # We can get the joint values from the group and adjust some of the values: # joint_goal[1] = 0 # joint_goal[2] = 0 # joint_goal[3] = 0 # joint_goal[4] = 0 # joint_goal[5] = 0 # The go command can be called with joint values, poses, or without any # parameters if you have already set the pose or joint target for the group # Calling ``stop()`` ensures that there is no residual movement ## END_SUB_TUTORIAL # For testing: # Copy class variables to local variables to make the web tutorials more clear. # In practice, you should use the class variables directly unless you have a good # reason not to. ## BEGIN_SUB_TUTORIAL plan_to_joint_state ## ## Planning to a Joint Goal ## ^^^^^^^^^^^^^^^^^^^^^^^^ ## The Panda's zero configuration is at a `singularity <https://www.quora.com/Robotics-What-is-meant-by-kinematic-singularity>`_ so the first ## thing we want to do is move it to a slightly better configuration. # We can get the joint values from the group and adjust some of the values: # The go command can be called with joint values, poses, or without any # parameters if you have already set the pose or joint target for the group # Calling ``stop()`` ensures that there is no residual movement ## END_SUB_TUTORIAL # For testing: # Copy class variables to local variables to make the web tutorials more clear. # In practice, you should use the class variables directly unless you have a good # reason not to. ## Now, we call the planner to compute the plan and execute it. # Calling `stop()` ensures that there is no residual movement # It is always good to clear your targets after planning with poses. # Note: there is no equivalent function for clear_joint_value_targets() ## END_SUB_TUTORIAL # For testing: # Note that since this section of code will not be included in the tutorials # we use the class variable rather than the copied state variable # Copy class variables to local variables to make the web tutorials more clear. # In practice, you should use the class variables directly unless you have a good # reason not to. ## BEGIN_SUB_TUTORIAL plan_cartesian_path ## ## Cartesian Paths ## ^^^^^^^^^^^^^^^ ## You can plan a Cartesian path directly by specifying a list of waypoints ## for the end-effector to go through. If executing  interactively in a ## Python shell, set scale = 1.0. ## # First move up (z) # and sideways (y) # Second move forward/backwards in (x) # Third move sideways (y) # We want the Cartesian path to be interpolated at a resolution of 1 cm # which is why we will specify 0.01 as the eef_step in Cartesian # translation.  We will disable the jump threshold by setting it to 0.0, # ignoring the check for infeasible jumps in joint space, which is sufficient # for this tutorial. # waypoints to follow # eef_step # jump_threshold # Note: We are just planning, not asking move_group to actually move the robot yet: ## END_SUB_TUTORIAL # Copy class variables to local variables to make the web tutorials more clear. # In practice, you should use the class variables directly unless you have a good # reason not to. ## BEGIN_SUB_TUTORIAL display_trajectory ## ## Displaying a Trajectory ## ^^^^^^^^^^^^^^^^^^^^^^^ ## You can ask RViz to visualize a plan (aka trajectory) for you. But the ## group.plan() method does this automatically so this is not that useful ## here (it just displays the same trajectory again): ## ## A `DisplayTrajectory`_ msg has two primary fields, trajectory_start and trajectory. ## We populate the trajectory_start with our current robot state to copy over ## any AttachedCollisionObjects and add our plan to the trajectory. # Publish ## END_SUB_TUTORIAL # Copy class variables to local variables to make the web tutorials more clear. # In practice, you should use the class variables directly unless you have a good # reason not to. ## BEGIN_SUB_TUTORIAL execute_plan ## ## Executing a Plan ## ^^^^^^^^^^^^^^^^ ## Use execute if you would like the robot to follow ## the plan that has already been computed: ## **Note:** The robot's current joint state must be within some tolerance of the ## first waypoint in the `RobotTrajectory`_ or ``execute()`` will fail ## END_SUB_TUTORIAL # Copy class variables to local variables to make the web tutorials more clear. # In practice, you should use the class variables directly unless you have a good # reason not to. ## BEGIN_SUB_TUTORIAL wait_for_scene_update ## ## Ensuring Collision Updates Are Received ## ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ ## If the Python node dies before publishing a collision object update message, the message ## could get lost and the box will not appear. To ensure that the updates are ## made, we wait until we see the changes reflected in the ## ``get_attached_objects()`` and ``get_known_object_names()`` lists. ## For the purpose of this tutorial, we call this function after adding, ## removing, attaching or detaching an object in the planning scene. We then wait ## until the updates have been made or ``timeout`` seconds have passed # Test if the box is in attached objects # Test if the box is in the scene. # Note that attaching the box will remove it from known_objects # Test if we are in the expected state # Sleep so that we give other threads time on the processor # If we exited the while loop without returning then we timed out ## END_SUB_TUTORIAL