ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question

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

asked 2019-04-29 06:22:48 -0500

kump gravatar image

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):
        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() )


        print("New Goal Orientation Tolerance: %f ...
edit retag flag offensive close merge delete


@kump I am having the same problem, was you able to solve it'?

Rony_s gravatar image Rony_s  ( 2019-11-05 06:07:19 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted

answered 2020-01-08 11:31:06 -0500

srm.robot gravatar image

I also had this issue with going to a pose goal (following the Tutorial for using the Python moveit commander) and always received "ABORTED: No motion plan found. "

What solved this issue for me was replacing pose_goal = geometry_msgs.msg.Pose() by pose_goal = geometry_msgs.msg.PoseStamped() (and copying the header frame from the current pose) like this (note the additional "pose" in the variable name when setting values as PoseStamped consists of Pose and Header):

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

Hope this helps someone, even though this might come too late for you now.


edit flag offensive delete link more


I changed the code as above. But I am not able to solve the same error as in question. Is it some sort of planning problem?

ankith_dh gravatar image ankith_dh  ( 2021-05-30 15:12:10 -0500 )edit

Question Tools

1 follower


Asked: 2019-04-29 06:22:48 -0500

Seen: 1,579 times

Last updated: Apr 29 '19