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 NOIKSOLUTION=-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
Asked by WarTurtle on 2023-03-22 08:15:13 UTC
Answers
It seems the FlexBE side is working if you are getting response. Be sure to confirm pose setting with respect to time and frame when making the service request. Check the MoveIt terminal for any information about why it failed.
Asked by dcconner on 2023-06-15 21:29:26 UTC
Comments