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

MoveIt /compute_ik gives no solution (error -31)

asked 2023-03-22 08:15:13 -0500

WarTurtle gravatar image

updated 2023-03-22 08:16:29 -0500

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
edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
0

answered 2023-06-15 21:29:26 -0500

dcconner gravatar image

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.

edit flag offensive delete link more

Question Tools

2 followers

Stats

Asked: 2023-03-22 08:15:13 -0500

Seen: 159 times

Last updated: Jun 15 '23