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" % self.move_group.get_goal_orientation_tolerance() )
        print("New Goal Position Tolerance: %f" % self.move_group.get_goal_position_tolerance() )


    def go_to_joint_state(self):
        print("Starting planning to go to joint state.\n")
        joint_goal = self.move_group.get_current_joint_values()
        joint_goal[0] = 0.7       # forearm 0
        joint_goal[1] = -0.5    # forearm 1
        joint_goal[2] = -0.7       # arm 0
        joint_goal[3] = 0.6527  # arm 1

        self.move_group.go(joint_goal, wait=True)

        self.move_group.stop()


    def go_to_home_joint_state(self):
        print("Starting planning to go to home joint state.\n")
        joint_goal = self.move_group.get_current_joint_values()
        joint_goal[0] = 0   # forearm 0
        joint_goal[1] = 0   # forearm 1
        joint_goal[2] = 0   # arm 0
        joint_goal[3] = 0   # arm 1

        self.move_group.go(joint_goal, wait=True)

        self.move_group.stop()


    def go_to_pose_goal(self):
        print("Starting planning to go to pose goal.\n")
        pose_goal = geometry_msgs.msg.Pose()
        pose_goal.orientation.w = 0.9
        pose_goal.orientation.x = 0.155
        pose_goal.orientation.y = 0.127
        pose_goal.orientation.z = 0.05
        pose_goal.position.x = -0.1
        pose_goal.position.y = -0.518
        pose_goal.position.z = 1.655

        self.move_group.set_pose_target(pose_goal)

        plan = self.move_group.go(wait=True)
        self.move_group.stop()
        self.move_group.clear_pose_targets()

        current_pose = self.move_group.get_current_pose().pose
        return all_close(pose_goal, current_pose, 0.1)


    def go_to_home_pose_goal(self):
        print("Starting planning to go to home pose goal.\n")
        pose_goal = geometry_msgs.msg.Pose()
        pose_goal.orientation.x = 0
        pose_goal.orientation.y = 0
        pose_goal.orientation.z = 0
        pose_goal.orientation.w = 1
        pose_goal.position.x = 0
        pose_goal.position.y = 0
        pose_goal.position.z = 1.835

        self.move_group.set_pose_target(pose_goal)

        plan = self.move_group.go(wait=True)
        self.move_group.stop()
        self.move_group.clear_pose_targets()

        current_pose = self.move_group.get_current_pose().pose
        return all_close(pose_goal, current_pose, 0.01)


    def execute_plan(self, plan):
        self.move_group.execute(plan, wait=True)


def main():
    moveit_context = MoveItContext()

    moveit_context.go_to_joint_state()
    moveit_context.go_to_home_joint_state()

    moveit_context.go_to_pose_goal()
    moveit_context.go_to_home_pose_goal()


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

script terminal:

Failed to import pyassimp, see https://github.com/ros-planning/moveit/issues/86 for more info
[ INFO] [1556532867.333204585]: Loading robot model 'robotic_arm'...
[ WARN] [1556532867.333433486]: Could not identify parent group for end-effector 'gripper_link'
[ INFO] [1556532867.399123636, 8108.100000000]: Loading robot model 'robotic_arm'...
[ WARN] [1556532867.399289097, 8108.100000000]: Could not identify parent group for end-effector 'gripper_link'
[ INFO] [1556532868.481820447, 8109.180000000]: Ready to take commands for planning group arm.
Default Goal Orientation Tolerance: 0.001000
Default Goal Position Tolerance: 0.000100
New Goal Orientation Tolerance: 0.500000
New Goal Position Tolerance: 0.050000
Starting planning to go to joint state.

Starting planning to go to home joint state.

Starting planning to go to pose goal.

[ INFO] [1556532877.360274416, 8118.044000000]: ABORTED: No motion plan found. No execution attempted.
Starting planning to go to home pose goal.

moveit terminal:

[ INFO] [1556532868.700731157, 8109.398000000]: Combined planning and execution request received for MoveGroup action. Forwarding to planning and execution pipeline.
[ INFO] [1556532868.700918955, 8109.398000000]: Planning attempt 1 of at most 1
[ INFO] [1556532868.701531152, 8109.399000000]: Planner configuration 'arm' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed.
[ INFO] [1556532868.701851687, 8109.399000000]: RRTConnect: Starting planning with 1 states already in datastructure
[ INFO] [1556532868.713508507, 8109.411000000]: RRTConnect: Created 4 states (2 start + 2 goal)
[ INFO] [1556532868.713636614, 8109.411000000]: Solution found in 0.011814 seconds
[ INFO] [1556532868.715724254, 8109.411000000]: SimpleSetup: Path simplification took 0.001998 seconds and changed from 3 to 2 states
[ INFO] [1556532870.484395986, 8111.178000000]: Completed trajectory execution with status SUCCEEDED ...
[ INFO] [1556532870.518828691, 8111.212000000]: Received event 'stop'
[ INFO] [1556532870.524751938, 8111.218000000]: Combined planning and execution request received for MoveGroup action. Forwarding to planning and execution pipeline.
[ INFO] [1556532870.524849814, 8111.218000000]: Planning attempt 1 of at most 1
[ INFO] [1556532870.525297859, 8111.218000000]: Planner configuration 'arm' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed.
[ INFO] [1556532870.525469181, 8111.218000000]: RRTConnect: Starting planning with 1 states already in datastructure
[ INFO] [1556532870.536461432, 8111.230000000]: RRTConnect: Created 4 states (2 start + 2 goal)
[ INFO] [1556532870.536516386, 8111.230000000]: Solution found in 0.011092 seconds
[ INFO] [1556532870.538040488, 8111.231000000]: SimpleSetup: Path simplification took 0.001480 seconds and changed from 3 to 2 states
[ INFO] [1556532872.268752212, 8112.958000000]: Completed trajectory execution with status SUCCEEDED ...
[ INFO] [1556532872.342864772, 8113.032000000]: Received event 'stop'
[ INFO] [1556532872.349163728, 8113.038000000]: Combined planning and execution request received for MoveGroup action. Forwarding to planning and execution pipeline.
[ WARN] [1556532872.349315003, 8113.038000000]: Orientation constraint for link 'gripper_link' is probably incorrect: 0.155000, 0.127000, 0.050000, 0.900000. Assuming identity instead.
[ INFO] [1556532872.349483622, 8113.038000000]: Planning attempt 1 of at most 1
[ WARN] [1556532872.349900502, 8113.039000000]: Orientation constraint for link 'gripper_link' is probably incorrect: 0.155000, 0.127000, 0.050000, 0.900000. Assuming identity instead.
[ WARN] [1556532872.350055858, 8113.039000000]: Orientation constraint for link 'gripper_link' is probably incorrect: 0.155000, 0.127000, 0.050000, 0.900000. Assuming identity instead.
[ INFO] [1556532872.350296396, 8113.039000000]: Planner configuration 'arm' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed.
[ INFO] [1556532872.350568239, 8113.039000000]: RRTConnect: Starting planning with 1 states already in datastructure
[ERROR] [1556532877.359794014, 8118.044000000]: RRTConnect: Unable to sample any valid states for goal tree
[ INFO] [1556532877.359852557, 8118.044000000]: RRTConnect: Created 1 states (1 start + 0 goal)
[ INFO] [1556532877.359882525, 8118.044000000]: No solution found after 5.009397 seconds
[ INFO] [1556532877.360056613, 8118.044000000]: Unable to solve the planning problem
[ INFO] [1556532877.360447004, 8118.044000000]: Received event 'stop'
[ INFO] [1556532877.394432087, 8118.078000000]: Combined planning and execution request received for MoveGroup action. Forwarding to planning and execution pipeline.
[ INFO] [1556532877.394530620, 8118.078000000]: Goal constraints are already satisfied. No need to plan or execute any motions
[ INFO] [1556532877.394755538, 8118.078000000]: Received event 'stop'

def go_to_pose_goal(self):
    print("Starting planning to go to pose goal.\n")
    pose_goal = geometry_msgs.msg.PoseStamped()
    pose_goal.header = actual_pose.header.seq
    pose_goal.header.stamp = actual_pose.header.stamp
    pose_goal.header.frame_id = ref_frame 
    pose_goal.pose.orientation.w = 0.9
    pose_goal.pose.orientation.x = 0.155
    pose_goal.pose.orientation.y = 0.127
    pose_goal.pose.orientation.z = 0.05
    pose_goal.pose.position.x = -0.1
    pose_goal.pose.position.y = -0.518
    pose_goal.pose.position.z = 1.655