MoveIt /compute_ik gives no solution (error -31)
I'm trying to the joint values for a robot arm given a specific target pose of an object. The code is below in FlexBe state format and gives Error code NO_IK_SOLUTION=-31. The full print of the result can be seen in the end. Hope you can help me :)
class MoveItIKState(EventState):
'''
Computes the joint configuration needed to grasp the part given its pose.
-- move_group string[] Name of group to be moved
-- offset float z-offset above the object
-- end_effector string end-effector link
># pose PoseStamped Pose of the part to pick
#> joint_values float[] Joint values for grasping
#> joint_names string[] Names of the joints
<= continue If a grasp configuration has been computed for the pose
<= failed otherwise.
'''
def __init__(self, move_group, offset, end_effector):
super(MoveItIKState, self).__init__(outcomes = ['continue', 'failed'], input_keys = ['pose'], output_keys = ['joint_values','joint_names'])
self._move_group = move_group
self._offset = offset
self._end_effector = end_effector
self._service_available = False
self._service_name = '/compute_ik'
self._tf_buffer = tf2_ros.Buffer(rospy.Duration(10.0)) # Buffer length
self._tf_listener = tf2_ros.TransformListener(self._tf_buffer)
try:
rospy.wait_for_service('/compute_ik', timeout=5.0)
self._compute_ik_service = ProxyServiceCaller({self._service_name: GetPositionIK})
self._service_available = True
except rospy.ServiceException as exception:
Logger.logerr("Service call failed: {}".format(exception))
return 'failed'
def execute(self, userdata):
if self._service_available == False:
Logger.logerr("Service is not available, state failed...")
return 'failed'
print(self._service_result)
def on_enter(self, userdata):
try:
self._target_pose = self._tf_buffer.transform(userdata.pose, "world")
self._target_pose.pose.position.z += self._offset
except Exception as exception:
Logger.logerr("Could not transform to world pose! " % exception)
return 'failed'
self._service_request = GetPositionIKRequest()
self._service_request.ik_request.group_name = self._move_group
self._service_request.ik_request.robot_state.joint_state = rospy.wait_for_message('/joint_states', sensor_msgs.msg.JointState)
self._service_request.ik_request.ik_link_name = self._end_effector
self._service_request.ik_request.pose_stamped = self._target_pose
self._service_request.ik_request.avoid_collisions = True
try:
self._service_result = self._compute_ik_service.call(self._service_name, self._service_request)
except Exception as exception:
rospy.logwarn("ERROR 1")
Logger.logerr("Could not call IK: " + str(exception))
return 'failed'
Output of self._service_result
:
solution:
joint_state:
header:
seq: 0
stamp:
secs: 0
nsecs: 0
frame_id: ''
name: []
position: []
velocity: []
effort: []
multi_dof_joint_state:
header:
seq: 0
stamp:
secs: 0
nsecs: 0
frame_id: ''
joint_names: []
transforms: []
twist: []
wrench: []
attached_collision_objects: []
is_diff: False
error_code:
val: -31