How to use the Python MoveIt! Interface to Control Pose of the End Effector?
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 ...
@kump I am having the same problem, was you able to solve it'?