def createExecuteKnownTrajectoryRequest(trajectory, wait_for_execution=True):
    """Create a ExecuteKnownTrajectoryRequest from the given data,
    trajectory must be a RobotTrajectory probably filled from a GetCartesianPath call"""
    ektr = ExecuteKnownTrajectoryRequest()
    ektr.trajectory = trajectory
    ektr.wait_for_execution = wait_for_execution
    return ektr
def createExecuteKnownTrajectoryRequest(trajectory, wait_for_execution=True):
    """Create a ExecuteKnownTrajectoryRequest from the given data,
    trajectory must be a RobotTrajectory probably filled from a GetCartesianPath call"""
    ektr = ExecuteKnownTrajectoryRequest()
    ektr.trajectory = trajectory
    ektr.wait_for_execution = wait_for_execution
    return ektr
    def on_enter(self, userdata):
        self._failed = False

        request = ExecuteKnownTrajectoryRequest()
        request.trajectory.joint_trajectory = userdata.joint_trajectory
        request.wait_for_execution = True

        try:
            self._result = self._srv.call(self._topic, request)
        except Exception as e:
            Logger.logwarn("Was unable to execute joint trajectory:\n%s" % str(e))
            self._failed = True
Example #4
0
def execute_trajectory(trajectory, wait_for_execution=True):

    req = ExecuteKnownTrajectoryRequest()
    req.trajectory = RobotTrajectory()
    req.trajectory.joint_trajectory = trajectory
    req.wait_for_execution = wait_for_execution

    srv = rospy.ServiceProxy('/execute_kinematic_path', ExecuteKnownTrajectory)
    res = srv(req)

    if wait_for_execution:
        rospy.sleep(1)

    return res.error_code
Example #5
0
 def go_to_pose(self,
                joint_names,
                positions,
                time=3.0,
                wait_for_execution=True):
     req = ExecuteKnownTrajectoryRequest()
     req.trajectory.joint_trajectory.joint_names = joint_names
     p = JointTrajectoryPoint()
     p.positions = positions
     p.time_from_start = rospy.Duration(time)
     req.trajectory.joint_trajectory.points.append(p)
     req.wait_for_execution = wait_for_execution
     try:
         resp = self.ekt_srv.call(req)
         return resp
     except rospy.ServiceException as e:
         rospy.logerr("Service exception: " + str(e))
Example #6
0
def execute(trajectory, wait=True, persistent=False, wait_timeout=None):
    """Call set robot speed service

    INPUT
        trajectory      [RobotTrajectory]
        wait            [Bool]
        persistent      [Bool, default=False]
        wait_timeout    [Float, default=None]

    OUTPUT
        response        [SetSpeedResponse]
    """
    srv_proxy = get_service_proxy(SRV_EXECUTE, ExecuteKnownTrajectory,
                                  persistent, wait_timeout)
    req = ExecuteKnownTrajectoryRequest()
    req.trajectory = trajectory
    req.wait_for_execution = wait
    res = srv_proxy(req)
    return res
    def go_to_configuration(self,
                            joint_names,
                            positions,
                            time=None,
                            wait_for_execution=None):
        """
        Execute a trajectory from current configuration to
        the given configuration.

        :param list of str joint_names: List of joint names.
        :param list of float positions: List of positions.
        """
        if len(joint_names) != len(positions):
            rospy.logerr("joint_names and positions have different lengths!")
            resp = ExecuteKnownTrajectoryResponse()
            resp.error_code = 9999  # Failure
            return resp
        if time is None:
            time = self.time
        if wait_for_execution is None:
            wait_for_execution = self.wait_for_execution
        req = ExecuteKnownTrajectoryRequest()
        req.trajectory.joint_trajectory.joint_names = joint_names
        p = JointTrajectoryPoint()
        p.positions = positions
        p.time_from_start = rospy.Duration(time)
        req.trajectory.joint_trajectory.points.append(p)
        req.wait_for_execution = wait_for_execution
        try:
            resp = self.ekt_srv.call(req)
            return resp
        except rospy.ServiceException as e:
            rospy.logerr("Service exception: " + str(e))
            resp = ExecuteKnownTrajectoryResponse()
            resp.error_code = 9999  # Failure
            return resp
    def on_enter(self, userdata):
        self._param_error = False
        self._request_failed = False
        self._moveit_failed = False
        self._success = False

        # Parameter check
        if self._srdf_param is None:
            self._param_error = True
            return

        try:
            self._srdf = ET.fromstring(self._srdf_param)
        except Exception as e:
            Logger.logwarn(
                'Unable to parse given SRDF parameter: /robot_description_semantic'
            )
            self._param_error = True

        if not self._param_error:

            robot = None
            for r in self._srdf.iter('robot'):
                if self._robot_name == '' or self._robot_name == r.attrib[
                        'name']:
                    robot = r
                    break
            if robot is None:
                Logger.logwarn('Did not find robot name in SRDF: %s' %
                               self._robot_name)
                return 'param_error'

            config = None
            for c in robot.iter('group_state'):
                if (self._move_group == '' or self._move_group == c.attrib['group']) \
                        and c.attrib['name'] == self._config_name:
                    config = c
                    self._move_group = c.attrib[
                        'group']  #Set move group name in case it was not defined
                    break
            if config is None:
                Logger.logwarn('Did not find config name in SRDF: %s' %
                               self._config_name)
                return 'param_error'

            try:
                self._joint_config = [
                    float(j.attrib['value']) for j in config.iter('joint')
                ]
                self._joint_names = [
                    str(j.attrib['name']) for j in config.iter('joint')
                ]
            except Exception as e:
                Logger.logwarn('Unable to parse joint values from SRDF:\n%s' %
                               str(e))
                return 'param_error'

            # Action Initialization
            action_goal = ExecuteKnownTrajectoryRequest(
            )  # ExecuteTrajectoryGoal()
            #action_goal.trajectory.joint_trajectory.header.stamp = rospy.Time.now() + rospy.Duration(0.3)
            action_goal.trajectory.joint_trajectory.joint_names = self._joint_names
            action_goal.trajectory.joint_trajectory.points = [
                JointTrajectoryPoint()
            ]
            action_goal.trajectory.joint_trajectory.points[
                0].time_from_start = rospy.Duration(self._duration)
            action_goal.wait_for_execution = self._wait_for_execution

            action_goal.trajectory.joint_trajectory.points[
                0].positions = self._joint_config

            try:
                self._response = self._client.call(self._action_topic,
                                                   action_goal)
                Logger.loginfo(
                    "Execute Known Trajectory Service requested: \n" +
                    str(self._action_topic))
            except rospy.ServiceException as exc:
                Logger.logwarn(
                    "Execute Known Trajectory Service did not process request: \n"
                    + str(exc))
                self._request_failed = True
                return
Example #9
0
def move_group_python_interface_tutorial():
    ## BEGIN_TUTORIAL
    ##
    ## Setup
    ## ^^^^^
    ## CALL_SUB_TUTORIAL imports
    ##
    ## First initialize moveit_commander and rospy.
    print "============ Starting tutorial setup"
    moveit_commander.roscpp_initialize(sys.argv)
    rospy.init_node('move_group_python_interface_tutorial',
                    anonymous=True)
    
    ## Instantiate a RobotCommander object.  This object is an interface to
    ## the robot as a whole.
    robot = moveit_commander.RobotCommander()
    
    ## Instantiate a PlanningSceneInterface object.  This object is an interface
    ## to the world surrounding the robot.
    scene = moveit_commander.PlanningSceneInterface()
    
    ## Instantiate a MoveGroupCommander object.  This object is an interface
    ## to one group of joints.  In this case the group is the joints in the left
    ## arm.  This interface can be used to plan and execute motions on the left
    ## arm.
    group = moveit_commander.MoveGroupCommander("right_arm")
    #group.set_planner_id("PRMkConfigDefault")
    
    
    ## We create this DisplayTrajectory publisher which is used below to publish
    ## trajectories for RVIZ to visualize.
    display_trajectory_publisher = rospy.Publisher(
                                        '/move_group/display_planned_path',
                                        moveit_msgs.msg.DisplayTrajectory)
    
    
    ## Getting Basic Information
    ## ^^^^^^^^^^^^^^^^^^^^^^^^^
    ##
    ## We can get the name of the reference frame for this robot
    print "============ Reference frame: %s" % group.get_planning_frame()
    
    ## We can also print the name of the end-effector link for this group
    print "============ Reference frame: %s" % group.get_end_effector_link()
    
    ## We can get a list of all the groups in the robot
    print "============ Robot Groups:"
    print robot.get_group_names()
    
    ## Sometimes for debugging it is useful to print the entire state of the
    ## robot.
    print "============ Printing robot state"
    print robot.get_current_state()
    print "============"
    
    rospy.sleep(5)
    print "Going to compute a cartesian path from where we are to z + 0.3"
    
    ## Cartesian Paths
    ## ^^^^^^^^^^^^^^^
    ## You can plan a cartesian path directly by specifying a list of waypoints 
    ## for the end-effector to go through.
    waypoints = []
    
    # start with the current pose
    rospy.sleep(1)
    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
    wpose.position.y = waypoints[0].position.y
    wpose.position.z = waypoints[0].position.z + 0.15
    waypoints.append(copy.deepcopy(wpose))
    
    
    # fourth move to the side a lot
    wpose.position.z += 0.15
    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 specify the jump threshold as 0.0, effectively
    ## disabling it.
    fraction = 0.0
    jump_threshold = 0
    eef_step = 0.01
    while fraction < 0.9:
        (plan3, fraction) = group.compute_cartesian_path(
                                     waypoints,   # waypoints to follow
                                     eef_step,        # eef_step
                                     jump_threshold)         # jump_threshold
        print "Fraction is: " + str(fraction) + " if it's less than 0.9 we will continue calculating"
        print "changing jump_threshold + 10 it was: " + str(jump_threshold)
        print "(Seems to make no difference)"
        jump_threshold += 10
        print "eef-step is: " + str(eef_step) + " adding 1cm"
        eef_step += 0.01
    
                                 
    
    print "plan looks like: " + str(plan3)
    print "with fraction being: " + str(fraction)
    print "Fraction should be 1, less than that, is that we went out of the straight line, i think"
    
    print "Moving arm to cartesian path thing"
    #group.go(wait=True)
    
    rospy.sleep(1)
    
    print "this did for sure not work, so lets try with executeknowntrajectory thing"
    ekp = rospy.ServiceProxy('/execute_kinematic_path', ExecuteKnownTrajectory)
    ekp.wait_for_service()
    
    print "!!!! Gonna send goal to execute_kinematic_path (waiting 3s)"
    rospy.sleep(3)
    ektr = ExecuteKnownTrajectoryRequest()
    ektr.trajectory = plan3
    ektr.wait_for_execution = True
    print "Sending call"
    ekp.call(ektr)
    print "!!!! Call done"
    
    
    ## Adding/Removing Objects and Attaching/Detaching Objects
    ## ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
    ## First, we will define the collision object message
    collision_object = moveit_msgs.msg.CollisionObject()
    
    
    
    ## When finished shut down moveit_commander.
    moveit_commander.roscpp_shutdown()
    
    ## END_TUTORIAL
    
    print "============ STOPPING"
def move_group_python_interface_tutorial():
    ## BEGIN_TUTORIAL
    ##
    ## Setup
    ## ^^^^^
    ## CALL_SUB_TUTORIAL imports
    ##
    ## First initialize moveit_commander and rospy.
    print "============ Starting tutorial setup"
    moveit_commander.roscpp_initialize(sys.argv)
    rospy.init_node('move_group_python_interface_tutorial', anonymous=True)

    ## Instantiate a RobotCommander object.  This object is an interface to
    ## the robot as a whole.
    robot = moveit_commander.RobotCommander()

    ## Instantiate a PlanningSceneInterface object.  This object is an interface
    ## to the world surrounding the robot.
    scene = moveit_commander.PlanningSceneInterface()

    ## Instantiate a MoveGroupCommander object.  This object is an interface
    ## to one group of joints.  In this case the group is the joints in the left
    ## arm.  This interface can be used to plan and execute motions on the left
    ## arm.
    group = moveit_commander.MoveGroupCommander("right_arm")
    #group.set_planner_id("PRMkConfigDefault")

    ## We create this DisplayTrajectory publisher which is used below to publish
    ## trajectories for RVIZ to visualize.
    display_trajectory_publisher = rospy.Publisher(
        '/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory)

    ## Getting Basic Information
    ## ^^^^^^^^^^^^^^^^^^^^^^^^^
    ##
    ## We can get the name of the reference frame for this robot
    print "============ Reference frame: %s" % group.get_planning_frame()

    ## We can also print the name of the end-effector link for this group
    print "============ Reference frame: %s" % group.get_end_effector_link()

    ## We can get a list of all the groups in the robot
    print "============ Robot Groups:"
    print robot.get_group_names()

    ## Sometimes for debugging it is useful to print the entire state of the
    ## robot.
    print "============ Printing robot state"
    print robot.get_current_state()
    print "============"

    ## Planning to a Pose goal
    ## ^^^^^^^^^^^^^^^^^^^^^^^
    ## We can plan a motion for this group to a desired pose for the
    ## end-effector
    print "============ Generating plan to initial pose"
    pose_target = geometry_msgs.msg.Pose()
    pose_target.orientation.w = 1.0
    pose_target.position.x = 0.3
    pose_target.position.y = -0.3
    pose_target.position.z = 1.1
    group.set_pose_target(pose_target)
    plan1 = group.plan()

    # Uncomment below line when working with a real robot
    print "Moving arm to initial 3d pose: " + str(pose_target)
    group.go(wait=True)
    rospy.sleep(1)
    group.clear_pose_targets()

    print "============ Generating plan to final pose"
    pose_target = geometry_msgs.msg.Pose()
    pose_target.orientation.w = 1.0
    pose_target.position.x = 0.3
    pose_target.position.y = -0.3
    pose_target.position.z = 1.4
    group.set_pose_target(pose_target)
    plan1 = group.plan()

    # Uncomment below line when working with a real robot
    print "Moving arm to final pose: " + str(pose_target)
    group.go(wait=True)
    rospy.sleep(1)
    group.clear_pose_targets()

    print "If everything worked well here, then we should be able to do a straight path"

    print "============ Generating plan to initial pose"
    pose_target = geometry_msgs.msg.Pose()
    pose_target.orientation.w = 1.0
    pose_target.position.x = 0.3
    pose_target.position.y = -0.3
    pose_target.position.z = 1.1
    group.set_pose_target(pose_target)
    plan1 = group.plan()

    # Uncomment below line when working with a real robot
    print "Moving arm to initial 3d pose: " + str(pose_target)
    group.go(wait=True)
    rospy.sleep(1)
    group.clear_pose_targets()

    ## Cartesian Paths
    ## ^^^^^^^^^^^^^^^
    ## You can plan a cartesian path directly by specifying a list of waypoints
    ## for the end-effector to go through.
    waypoints = []

    # start with the current pose
    rospy.sleep(1)
    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
    wpose.position.y = waypoints[0].position.y
    wpose.position.z = waypoints[0].position.z + 0.15
    waypoints.append(copy.deepcopy(wpose))

    # fourth move to the side a lot
    wpose.position.z += 0.15
    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 specify the jump threshold as 0.0, effectively
    ## disabling it.
    (plan3, fraction) = group.compute_cartesian_path(
        waypoints,  # waypoints to follow
        0.02,  # eef_step
        0.0)  # jump_threshold

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

    print "plan3 looks like: " + str(plan3)
    print "with fraction being: " + str(fraction)

    print "Moving arm to cartesian path thing"
    group.go(wait=True)

    print "this did for sure not work, so lets try with executeknowntrajectory thing"
    ekp = rospy.ServiceProxy('/execute_kinematic_path', ExecuteKnownTrajectory)
    ekp.wait_for_service()

    print "!!!! Gonna send goal to execute_kinematic_path"
    rospy.sleep(4)
    ektr = ExecuteKnownTrajectoryRequest()
    ektr.trajectory = plan3
    ektr.wait_for_execution = True
    ekp.call(ektr)
    print "!!!! Call done"

    ## Adding/Removing Objects and Attaching/Detaching Objects
    ## ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
    ## First, we will define the collision object message
    collision_object = moveit_msgs.msg.CollisionObject()

    ## When finished shut down moveit_commander.
    moveit_commander.roscpp_shutdown()

    ## END_TUTORIAL

    print "============ STOPPING"
def move_group_python_interface_tutorial():
    ## BEGIN_TUTORIAL
    ##
    ## Setup
    ## ^^^^^
    ## CALL_SUB_TUTORIAL imports
    ##
    ## First initialize moveit_commander and rospy.
    print "============ Starting tutorial setup"
    moveit_commander.roscpp_initialize(sys.argv)
    rospy.init_node('move_group_python_interface_tutorial', anonymous=True)

    ## Instantiate a RobotCommander object.  This object is an interface to
    ## the robot as a whole.
    robot = moveit_commander.RobotCommander()

    ## Instantiate a PlanningSceneInterface object.  This object is an interface
    ## to the world surrounding the robot.
    scene = moveit_commander.PlanningSceneInterface()

    ## Instantiate a MoveGroupCommander object.  This object is an interface
    ## to one group of joints.  In this case the group is the joints in the left
    ## arm.  This interface can be used to plan and execute motions on the left
    ## arm.
    right_group = moveit_commander.MoveGroupCommander("right_arm")
    left_group = moveit_commander.MoveGroupCommander("left_arm")
    #group.set_planner_id("PRMkConfigDefault")

    print "Going to compute a cartesian path for right arm from where we are to z + 0.3"

    ### Right part
    waypoints = []

    # start with the current pose
    rospy.sleep(1)
    waypoints.append(right_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
    wpose.position.y = waypoints[0].position.y
    wpose.position.z = waypoints[0].position.z + 0.15
    waypoints.append(copy.deepcopy(wpose))

    # fourth move to the side a lot
    wpose.position.z += 0.15
    waypoints.append(copy.deepcopy(wpose))

    (planright, fraction) = right_group.compute_cartesian_path(
        waypoints,  # waypoints to follow
        0.02,  # eef_step
        0.0)  # jump_threshold

    print "planright looks like: " + str(planright)
    print "with fraction being: " + str(fraction)

    #### left part
    waypoints = []

    # start with the current pose
    rospy.sleep(1)
    waypoints.append(left_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
    wpose.position.y = waypoints[0].position.y
    wpose.position.z = waypoints[0].position.z + 0.15
    waypoints.append(copy.deepcopy(wpose))

    # fourth move to the side a lot
    wpose.position.z += 0.15
    waypoints.append(copy.deepcopy(wpose))

    (planleft, fraction) = left_group.compute_cartesian_path(
        waypoints,  # waypoints to follow
        0.02,  # eef_step
        0.0)  # jump_threshold

    print "planleft looks like: " + str(planleft)
    print "with fraction being: " + str(fraction)

    ### EXECUTE STUFF
    rospy.sleep(1)

    finalplan = join_trajectories(planright, planleft)
    print "final plan looks like: " + str(finalplan)
    print "so lets try with executeknowntrajectory thing"
    ekp = rospy.ServiceProxy('/execute_kinematic_path', ExecuteKnownTrajectory)
    ekp.wait_for_service()

    print "!!!! Gonna send BOTH ARMS goal to execute_kinematic_path"
    #rospy.sleep(3)
    ektr = ExecuteKnownTrajectoryRequest()
    ektr.trajectory = finalplan
    ektr.wait_for_execution = True
    print "Sending call both arms"
    ekp.call(ektr)
    print "!!!! Call done"
    #
    #     print "!!!! Gonna send LEFT goal to execute_kinematic_path"
    #     #rospy.sleep(3)
    #     ektr = ExecuteKnownTrajectoryRequest()
    #     ektr.trajectory = planleft
    #     ektr.wait_for_execution = True
    #     print "Sending call left arm"
    #     ekp.call(ektr)
    #     print "!!!! Call done"

    ## When finished shut down moveit_commander.
    moveit_commander.roscpp_shutdown()

    ## END_TUTORIAL

    print "============ STOPPING"
def move_group_python_interface_tutorial():
    ## BEGIN_TUTORIAL
    ##
    ## Setup
    ## ^^^^^
    ## CALL_SUB_TUTORIAL imports
    ##
    ## First initialize moveit_commander and rospy.
    print "============ Starting tutorial setup"
    moveit_commander.roscpp_initialize(sys.argv)
    rospy.init_node('move_group_python_interface_tutorial',
                    anonymous=True)
    
    ## Instantiate a RobotCommander object.  This object is an interface to
    ## the robot as a whole.
    robot = moveit_commander.RobotCommander()
    
    ## Instantiate a PlanningSceneInterface object.  This object is an interface
    ## to the world surrounding the robot.
    scene = moveit_commander.PlanningSceneInterface()
    
    ## Instantiate a MoveGroupCommander object.  This object is an interface
    ## to one group of joints.  In this case the group is the joints in the left
    ## arm.  This interface can be used to plan and execute motions on the left
    ## arm.
    right_group = moveit_commander.MoveGroupCommander("right_arm")
    left_group = moveit_commander.MoveGroupCommander("left_arm")
    #group.set_planner_id("PRMkConfigDefault")
    

    print "Going to compute a cartesian path for right arm from where we are to z + 0.3"
    
    ### Right part
    waypoints = []
    
    # start with the current pose
    rospy.sleep(1)
    waypoints.append(right_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
    wpose.position.y = waypoints[0].position.y
    wpose.position.z = waypoints[0].position.z + 0.15
    waypoints.append(copy.deepcopy(wpose))
    
    
    # fourth move to the side a lot
    wpose.position.z += 0.15
    waypoints.append(copy.deepcopy(wpose))
    
    (planright, fraction) =right_group.compute_cartesian_path(
                                 waypoints,   # waypoints to follow
                                 0.02,        # eef_step
                                 0.0)         # jump_threshold
    
    
    print "planright looks like: " + str(planright)
    print "with fraction being: " + str(fraction)
    
    
    #### left part
    waypoints = []
    
    # start with the current pose
    rospy.sleep(1)
    waypoints.append(left_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
    wpose.position.y = waypoints[0].position.y
    wpose.position.z = waypoints[0].position.z + 0.15
    waypoints.append(copy.deepcopy(wpose))
    
    
    # fourth move to the side a lot
    wpose.position.z += 0.15
    waypoints.append(copy.deepcopy(wpose))
    
    (planleft, fraction) = left_group.compute_cartesian_path(
                                 waypoints,   # waypoints to follow
                                 0.02,        # eef_step
                                 0.0)         # jump_threshold
    
    
    print "planleft looks like: " + str(planleft)
    print "with fraction being: " + str(fraction)
    
    
    ### EXECUTE STUFF
    rospy.sleep(1)
    
    finalplan = join_trajectories(planright, planleft)
    print "final plan looks like: " + str(finalplan)
    print "so lets try with executeknowntrajectory thing"
    ekp = rospy.ServiceProxy('/execute_kinematic_path', ExecuteKnownTrajectory)
    ekp.wait_for_service()
     
    print "!!!! Gonna send BOTH ARMS goal to execute_kinematic_path"
    #rospy.sleep(3)
    ektr = ExecuteKnownTrajectoryRequest()
    ektr.trajectory = finalplan
    ektr.wait_for_execution = True
    print "Sending call both arms"
    ekp.call(ektr)
    print "!!!! Call done"
#     
#     print "!!!! Gonna send LEFT goal to execute_kinematic_path"
#     #rospy.sleep(3)
#     ektr = ExecuteKnownTrajectoryRequest()
#     ektr.trajectory = planleft
#     ektr.wait_for_execution = True
#     print "Sending call left arm"
#     ekp.call(ektr)
#     print "!!!! Call done"
    
    ## When finished shut down moveit_commander.
    moveit_commander.roscpp_shutdown()
    
    ## END_TUTORIAL
    
    print "============ STOPPING"
def move_group_python_interface_tutorial():
    ## BEGIN_TUTORIAL
    ##
    ## Setup
    ## ^^^^^
    ## CALL_SUB_TUTORIAL imports
    ##
    ## First initialize moveit_commander and rospy.
    print "============ Starting tutorial setup"
    moveit_commander.roscpp_initialize(sys.argv)
    rospy.init_node('move_group_python_interface_tutorial',
                    anonymous=True)
    
    ## Instantiate a RobotCommander object.  This object is an interface to
    ## the robot as a whole.
    robot = moveit_commander.RobotCommander()
    
    ## Instantiate a PlanningSceneInterface object.  This object is an interface
    ## to the world surrounding the robot.
    scene = moveit_commander.PlanningSceneInterface()
    
    ## Instantiate a MoveGroupCommander object.  This object is an interface
    ## to one group of joints.  In this case the group is the joints in the left
    ## arm.  This interface can be used to plan and execute motions on the left
    ## arm.
    group = moveit_commander.MoveGroupCommander("right_arm")
    #group.set_planner_id("PRMkConfigDefault")
    
    
    ## We create this DisplayTrajectory publisher which is used below to publish
    ## trajectories for RVIZ to visualize.
    display_trajectory_publisher = rospy.Publisher(
                                        '/move_group/display_planned_path',
                                        moveit_msgs.msg.DisplayTrajectory)
    
    
    ## Getting Basic Information
    ## ^^^^^^^^^^^^^^^^^^^^^^^^^
    ##
    ## We can get the name of the reference frame for this robot
    print "============ Reference frame: %s" % group.get_planning_frame()
    
    ## We can also print the name of the end-effector link for this group
    print "============ Reference frame: %s" % group.get_end_effector_link()
    
    ## We can get a list of all the groups in the robot
    print "============ Robot Groups:"
    print robot.get_group_names()
    
    ## Sometimes for debugging it is useful to print the entire state of the
    ## robot.
    print "============ Printing robot state"
    print robot.get_current_state()
    print "============"
    
    
    ## Planning to a Pose goal
    ## ^^^^^^^^^^^^^^^^^^^^^^^
    ## We can plan a motion for this group to a desired pose for the 
    ## end-effector
    print "============ Generating plan to initial pose"
    pose_target = geometry_msgs.msg.Pose()
    pose_target.orientation.w = 1.0
    pose_target.position.x = 0.3
    pose_target.position.y = -0.3
    pose_target.position.z = 1.1
    group.set_pose_target(pose_target)
    plan1 = group.plan()
    
    
    # Uncomment below line when working with a real robot
    print "Moving arm to initial 3d pose: " + str(pose_target)
    group.go(wait=True)
    rospy.sleep(1)
    group.clear_pose_targets()
        
    print "============ Generating plan to final pose"
    pose_target = geometry_msgs.msg.Pose()
    pose_target.orientation.w = 1.0
    pose_target.position.x = 0.3
    pose_target.position.y = -0.3
    pose_target.position.z = 1.4
    group.set_pose_target(pose_target)
    plan1 = group.plan()
    
    
    # Uncomment below line when working with a real robot
    print "Moving arm to final pose: " + str(pose_target)
    group.go(wait=True)
    rospy.sleep(1)
    group.clear_pose_targets()
    
    print "If everything worked well here, then we should be able to do a straight path"
    
    
    print "============ Generating plan to initial pose"
    pose_target = geometry_msgs.msg.Pose()
    pose_target.orientation.w = 1.0
    pose_target.position.x = 0.3
    pose_target.position.y = -0.3
    pose_target.position.z = 1.1
    group.set_pose_target(pose_target)
    plan1 = group.plan()

    # Uncomment below line when working with a real robot
    print "Moving arm to initial 3d pose: " + str(pose_target)
    group.go(wait=True)
    rospy.sleep(1)
    group.clear_pose_targets()
    
    ## Cartesian Paths
    ## ^^^^^^^^^^^^^^^
    ## You can plan a cartesian path directly by specifying a list of waypoints 
    ## for the end-effector to go through.
    waypoints = []
    
    # start with the current pose
    rospy.sleep(1)
    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
    wpose.position.y = waypoints[0].position.y
    wpose.position.z = waypoints[0].position.z + 0.15
    waypoints.append(copy.deepcopy(wpose))
    
    
    # fourth move to the side a lot
    wpose.position.z += 0.15
    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 specify the jump threshold as 0.0, effectively
    ## disabling it.
    (plan3, fraction) = group.compute_cartesian_path(
                                 waypoints,   # waypoints to follow
                                 0.02,        # eef_step
                                 0.0)         # jump_threshold
                                 
    print "============ Waiting while RVIZ displays plan3..."
    rospy.sleep(3)
    
    print "plan3 looks like: " + str(plan3)
    print "with fraction being: " + str(fraction)
    
    print "Moving arm to cartesian path thing"
    group.go(wait=True)
    
    
    print "this did for sure not work, so lets try with executeknowntrajectory thing"
    ekp = rospy.ServiceProxy('/execute_kinematic_path', ExecuteKnownTrajectory)
    ekp.wait_for_service()
    
    print "!!!! Gonna send goal to execute_kinematic_path"
    rospy.sleep(4)
    ektr = ExecuteKnownTrajectoryRequest()
    ektr.trajectory = plan3
    ektr.wait_for_execution = True
    ekp.call(ektr)
    print "!!!! Call done"
    
    
    ## Adding/Removing Objects and Attaching/Detaching Objects
    ## ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
    ## First, we will define the collision object message
    collision_object = moveit_msgs.msg.CollisionObject()
    
    
    
    ## When finished shut down moveit_commander.
    moveit_commander.roscpp_shutdown()
    
    ## END_TUTORIAL
    
    print "============ STOPPING"
    def on_enter(self, userdata):
        self._param_error    = False
        self._request_failed = False
        self._moveit_failed  = False
        self._success        = False

        # Parameter check
        if self._srdf_param is None:
            self._param_error = True
            return

        try:
            self._srdf = ET.fromstring(self._srdf_param)
        except Exception as e:
            Logger.logwarn('Unable to parse given SRDF parameter: /robot_description_semantic')
            self._param_error = True

        if not self._param_error:

            robot = None
            for r in self._srdf.iter('robot'):
                if self._robot_name == '' or self._robot_name == r.attrib['name']:
                    robot = r
                    break
            if robot is None:
                Logger.logwarn('Did not find robot name in SRDF: %s' % self._robot_name)
                return 'param_error'

            config = None
            for c in robot.iter('group_state'):
                if (self._move_group == '' or self._move_group == c.attrib['group']) \
                        and c.attrib['name'] == self._config_name:
                    config = c
                    self._move_group = c.attrib['group'] #Set move group name in case it was not defined
                    break
            if config is None:
                Logger.logwarn('Did not find config name in SRDF: %s' % self._config_name)
                return 'param_error'

            try:
                self._joint_config = [float(j.attrib['value']) for j in config.iter('joint')]
                self._joint_names = [str(j.attrib['name']) for j in config.iter('joint')]
            except Exception as e:
                Logger.logwarn('Unable to parse joint values from SRDF:\n%s' % str(e))
                return 'param_error'

            # Action Initialization
            action_goal = ExecuteKnownTrajectoryRequest()  # ExecuteTrajectoryGoal()
            #action_goal.trajectory.joint_trajectory.header.stamp = rospy.Time.now() + rospy.Duration(0.3)
            action_goal.trajectory.joint_trajectory.joint_names = self._joint_names
            action_goal.trajectory.joint_trajectory.points = [JointTrajectoryPoint()]
            action_goal.trajectory.joint_trajectory.points[0].time_from_start = rospy.Duration(self._duration)
            action_goal.wait_for_execution = self._wait_for_execution

            action_goal.trajectory.joint_trajectory.points[0].positions = self._joint_config

            try:
                self._response = self._client.call(self._action_topic, action_goal)
                Logger.loginfo("Execute Known Trajectory Service requested: \n" + str(self._action_topic))
            except rospy.ServiceException as exc:
                Logger.logwarn("Execute Known Trajectory Service did not process request: \n" + str(exc))
                self._request_failed = True
                return