How to use the Python MoveIt! Interface to Control Pose of the End Effector?

edit

I am using the robotic arm from this repository and trying to make a python script that would control the movement of the robot. I am using this tutorial to write the python script.

My script creates this MoveIt! class with moveit_commander objects and two functions for controlling the robot movement.

The first function controls the movement by setting the joint angles to each joint. This method works well.

The other method uses pose goal to control the arm. I have created two methods with this approach (same as for the one for joint states). One is supposed to move the robotic arm to some position different from the home (initial) position and the other is supposed to bring the robot arm back to the home position. I got the values for the pose goals by using the joint state methods to move the robotic arm to respective position and then calling the move_group.get_current_pose().pose method and printing it on the terminal. So the positions should be the same for both sets of move commands (joint state and position goal approaches).

And so the joint state approach methods executes correctly, but the pose goal approach methods does not and outputs some errors on the terminal, which are shown bellow. I even increased the goal position tolerance in order to make it easier to find the solution, as can be seen at the end of the __init__ method of the MoveItContext class. It made the pose goal method not to fail if the robot is already in the pose it is trying to plan for.

Any idea what is causing it and how to fix this?

The script:

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

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 MoveItContext(object):
    def __init__(self):
        moveit_commander.roscpp_initialize(sys.argv)
        rospy.init_node('robotic_arm_moveit', anonymous=True)

        robot = moveit_commander.RobotCommander()
        scene = moveit_commander.PlanningSceneInterface()
        group_name = "arm"
        move_group = moveit_commander.MoveGroupCommander(group_name)

        planning_frame = move_group.get_planning_frame()
        end_effector_link = move_group.get_end_effector_link()
        group_names = robot.get_group_names()

        self.box_name = ''
        self.robot = robot
        self.scene = scene
        self.move_group = move_group
        self.planning_frame = planning_frame
        self.end_effector_link = end_effector_link
        self.group_names = group_names

        print("Default Goal Orientation Tolerance: %f" % self.move_group.get_goal_orientation_tolerance() )
        print("Default Goal Position Tolerance: %f" % self.move_group.get_goal_position_tolerance() )

        self.move_group.set_goal_orientation_tolerance(0.5)
        self.move_group.set_goal_position_tolerance(0.05)

        print("New Goal Orientation Tolerance: %f ...