compute_ik service for open manipulator - response error -31 no soln found

asked 2021-06-07 08:26:19 -0500

querybot gravatar image

Hello everyone, i am starting an open maipulator x here which consists of two groups ("om_two/arm" and "om_two/gripper") with moveit, i have the pose stamp (position and orientation) of an arbitrary frame stored in an array (self.tag_frame_stampedpose) and would like to use the service /compute_ik to compute the joint pose for my arm group ("om_two/arm") such that the end effector of my arm is at the position of that frame. The service request for this purpose was filled as follows:

  def srvCallback(self):
    self.ar_pose_marker_sub = rospy.Subscriber(self.topic, ar_track_alvar_msgs.msg.AlvarMarkers, self.arPoseMarkerCallback)
    while self.isReady == False:
      rospy.loginfo("waiting for tag pose to become available")
      continue
    # ------ Preparing service request for "compute_ik" -------
    request = moveit_msgs.srv.GetPositionIKRequest() # init request
    # filling request with necessary parameters for IK computation
    request.ik_request.group_name = "om_two/arm"
    request.ik_request.pose_stamped.header.frame_id = "om_two/world"
    request.ik_request.pose_stamped.header.stamp = rospy.Time.now()
    request.ik_request.robot_state.joint_state.name = ["om_two/joint1", "om_two/joint2", "om_two/joint3", "om_two/joint4"] 
    request.ik_request.robot_state.joint_state.position = [1.7011847496032715, -1.0431069135665894, 0.029145635664463043, 1.7272623777389526]
    #request.ik_request.robot_state.is_diff = True
    request.ik_request.pose_stamped.pose.position.x = self.tag_frame_stampedpose[0]
    request.ik_request.pose_stamped.pose.position.y = self.tag_frame_stampedpose[1]
    request.ik_request.pose_stamped.pose.position.z = self.tag_frame_stampedpose[2]
    request.ik_request.pose_stamped.pose.orientation.x = self.tag_frame_stampedpose[3]
    request.ik_request.pose_stamped.pose.orientation.y = self.tag_frame_stampedpose[4]
    request.ik_request.pose_stamped.pose.orientation.z = self.tag_frame_stampedpose[5]
    request.ik_request.pose_stamped.pose.orientation.w = self.tag_frame_stampedpose[6]
    #-------------------------- call compute_ik service -------------------------------
    rospy.wait_for_service(self.ik_srv_name)
    try:
      ik_srv = rospy.ServiceProxy(self.ik_srv_name, moveit_msgs.srv.GetPositionIK)
      rospy.loginfo("service init")
      response = ik_srv(request)
      rospy.loginfo("service called")
      rospy.loginfo(response)
      rospy.set_param('joint_pose_to_ar_tag', response.solution.joint_state.position)
    except rospy.ServiceException, e:
      rospy.logwarn(e) #service call failed

I tried different stamp poses and also tried changing the frame_id from "om_two/world" to "om_two/end_effector_link" but i keep getting the same error

The suggestion from this issue also didn't solve the problem.

Am I overseeing something? Any advice be fully appreciated. Thanks in advance

edit retag flag offensive close merge delete