def _updateIKFeasibility(self, ps):
    p = PoseStamped()
    p.pose.orientation = ps.pose.orientation
    p.header.frame_id = '/torso_lift_link'
    offset = -0.216 #distance from tool frame to wrist_roll_link
    q = ps.pose.orientation
    rot = tr.tft.quaternion_matrix((q.x, q.y, q.z, q.w))
    offset_vec = rot * np.matrix([[offset, 0., 0., 1.]]).T
    p.pose.position.x = ps.pose.position.x + offset_vec[0]
    p.pose.position.y = ps.pose.position.y + offset_vec[1]
    p.pose.position.z = ps.pose.position.z + offset_vec[2]
    self.test_pos_pub.publish(p)

    req = GetPositionIKRequest()
    req.timeout = rospy.Duration(5.0)
    ik_req = PositionIKRequest()
    ik_req.ik_link_name = self.ik_solver_info.link_names[-1]
    ik_req.ik_seed_state.joint_state.name = self.ik_solver_info.joint_names
    ik_req.ik_seed_state.joint_state.position = self.joint_state
    ik_req.pose_stamped = ps
    req.ik_request = ik_req
    ik_sol = self.pr2_ik_client.call(req)
    if ik_sol.error_code.val < 0:
      print "IK Failed with error code: %s" %ik_sol.error_code.val
      for marker in self.wp_im.controls[0].markers:
        marker.color = ColorRGBA(1,0,0,0.7)
    else:
      for marker in self.wp_im.controls[0].markers:
        marker.color = ColorRGBA(0,1,0,0.7)
Esempio n. 2
0
    def get_ik(self, pose_stamped, collision_aware=True, starting_state=None,
               seed_state=None, timeout=5.0, ordered_collisions=None):
        '''
        Solves inverse kinematics problems.

        **Args:**

            **pose_stamped (geometry_msgs.msg.PoseStamped):** The pose for which to get joint positions

            *collision_aware (boolean):* If True, returns a solution that does not collide with anything in the world

            *starting_state (arm_navigation_msgs.msg.RobotState):* The returned state will have all non-arm joints
            matching this state.  If no state is passed in, it will use the current state in the planning scene
            interface.

            *seed_state (arm_navigation_msgs.msg.RobotState):* A seed state for IK.  If no state is passed it, it 
            will use the current state in planning scene interface.

            *timeout (double):* Time in seconds that IK is allowed to run

        **Returns:**
            A kinematics_msgs.srv.GetConstraintAwarePositionIKResponse if collision_aware was True and a
            kinematics_msgs.srv.GetPosiitonIKResponse if collision_aware was False.  The robot state returned has
            the arm joints set to the IK solution if found and all other joints set to that of starting_state.
            
        **Raises:**

            **rospy.ServiceException:** if there is a problem with the service call

        **Warning:**
            Calls an IK service which may use TF for transformations!  Probably best to only use with pose stampeds
            defined in the robot frame (convert them yourself using the planning scene interface).
        '''
        rospy.logdebug('Solving IK for\n'+str(pose_stamped))
        pos_req = PositionIKRequest()
        pos_req.ik_link_name = self.hand.hand_frame
        pos_req.pose_stamped = pose_stamped
        if not starting_state: 
            starting_state = self._psi.get_robot_state()
        if not seed_state:
            seed_state = self.arm_robot_state(starting_state)
        pos_req.ik_seed_state = seed_state
        pos_req.robot_state = starting_state
        if collision_aware:
            self._psi.add_ordered_collisions(ordered_collisions)
            coll_req = GetConstraintAwarePositionIKRequest()
            coll_req.ik_request = pos_req
            coll_req.timeout = rospy.Duration(timeout)
            coll_res = self._collision_aware_ik_service(coll_req)
            coll_res.solution = tt.set_joint_state_in_robot_state(coll_res.solution.joint_state,
                                                                  copy.deepcopy(starting_state))
            self._psi.remove_ordered_collisions(ordered_collisions)
            return coll_res
        coll_req = GetPositionIKRequest()
        coll_req.ik_request = pos_req
        coll_req.timeout = rospy.Duration(timeout)
        coll_res = self._ik_service(coll_req)
        coll_res.solution = tt.set_joint_state_in_robot_state(coll_res.solution.joint_state,
                                                              copy.deepcopy(starting_state))
        return coll_res
    def run_ik(
        self,
        pose_stamped,
        start_angles,
        link_name,
        collision_aware=1,
        ordered_collision_operations=None,
        IK_robot_state=None,
        link_padding=None,
    ):

        if link_name not in self.link_names:
            rospy.logerr("link name %s not possible!" % link_name)
            return None
        self.link_name = link_name

        # print "request:\n", pose_stamped

        ik_request = PositionIKRequest()
        ik_request.ik_link_name = self.link_name
        ik_request.pose_stamped = pose_stamped
        ik_request.ik_seed_state.joint_state.header.stamp = rospy.get_rostime()
        ik_request.ik_seed_state.joint_state.position = start_angles
        ik_request.ik_seed_state.joint_state.name = self.joint_names

        if IK_robot_state:
            ik_request.robot_state = IK_robot_state

        try:
            if collision_aware and self.perception_running:
                col_free_ik_request = GetConstraintAwarePositionIKRequest()
                col_free_ik_request.ik_request = ik_request
                col_free_ik_request.timeout = rospy.Duration(10.0)  # timeout after 10 seconds

                if ordered_collision_operations != None:
                    col_free_ik_request.ordered_collision_operations = ordered_collision_operations
                if link_padding != None:
                    col_free_ik_request.link_padding = link_padding

                resp = self._ik_service_with_collision(col_free_ik_request)
            else:
                resp = self._ik_service(ik_request, rospy.Duration(10.0))
        except rospy.ServiceException, e:
            rospy.logwarn("IK service call failed! error msg: %s" % e)
            return (None, None)
    def run_ik(self,
               pose_stamped,
               start_angles,
               link_name,
               collision_aware=1,
               ordered_collision_operations=None,
               IK_robot_state=None,
               link_padding=None):

        if link_name not in self.link_names:
            rospy.logerr("link name %s not possible!" % link_name)
            return None
        self.link_name = link_name

        #print "request:\n", pose_stamped

        ik_request = PositionIKRequest()
        ik_request.ik_link_name = self.link_name
        ik_request.pose_stamped = pose_stamped
        ik_request.ik_seed_state.joint_state.header.stamp = rospy.get_rostime()
        ik_request.ik_seed_state.joint_state.position = start_angles
        ik_request.ik_seed_state.joint_state.name = self.joint_names

        if IK_robot_state:
            ik_request.robot_state = IK_robot_state

        try:
            if collision_aware and self.perception_running:
                col_free_ik_request = GetConstraintAwarePositionIKRequest()
                col_free_ik_request.ik_request = ik_request
                col_free_ik_request.timeout = rospy.Duration(
                    10.0)  #timeout after 10 seconds

                if ordered_collision_operations != None:
                    col_free_ik_request.ordered_collision_operations = ordered_collision_operations
                if link_padding != None:
                    col_free_ik_request.link_padding = link_padding

                resp = self._ik_service_with_collision(col_free_ik_request)
            else:
                resp = self._ik_service(ik_request, rospy.Duration(10.0))
        except rospy.ServiceException, e:
            rospy.logwarn("IK service call failed! error msg: %s" % e)
            return (None, None)
Esempio n. 5
0
    def get_ik(self,
               pose_stamped,
               collision_aware=True,
               starting_state=None,
               seed_state=None,
               timeout=5.0,
               ordered_collisions=None):
        '''
        Solves inverse kinematics problems.

        **Args:**

            **pose_stamped (geometry_msgs.msg.PoseStamped):** The pose for which to get joint positions

            *collision_aware (boolean):* If True, returns a solution that does not collide with anything in the world

            *starting_state (arm_navigation_msgs.msg.RobotState):* The returned state will have all non-arm joints
            matching this state.  If no state is passed in, it will use the current state in the planning scene
            interface.

            *seed_state (arm_navigation_msgs.msg.RobotState):* A seed state for IK.  If no state is passed it, it 
            will use the current state in planning scene interface.

            *timeout (double):* Time in seconds that IK is allowed to run

        **Returns:**
            A kinematics_msgs.srv.GetConstraintAwarePositionIKResponse if collision_aware was True and a
            kinematics_msgs.srv.GetPosiitonIKResponse if collision_aware was False.  The robot state returned has
            the arm joints set to the IK solution if found and all other joints set to that of starting_state.
            
        **Raises:**

            **rospy.ServiceException:** if there is a problem with the service call

        **Warning:**
            Calls an IK service which may use TF for transformations!  Probably best to only use with pose stampeds
            defined in the robot frame (convert them yourself using the planning scene interface).
        '''
        rospy.logdebug('Solving IK for\n' + str(pose_stamped))
        pos_req = PositionIKRequest()
        pos_req.ik_link_name = self.hand.hand_frame
        pos_req.pose_stamped = pose_stamped
        if not starting_state:
            starting_state = self._psi.get_robot_state()
        if not seed_state:
            seed_state = self.arm_robot_state(starting_state)
        pos_req.ik_seed_state = seed_state
        pos_req.robot_state = starting_state
        if collision_aware:
            self._psi.add_ordered_collisions(ordered_collisions)
            coll_req = GetConstraintAwarePositionIKRequest()
            coll_req.ik_request = pos_req
            coll_req.timeout = rospy.Duration(timeout)
            coll_res = self._collision_aware_ik_service(coll_req)
            coll_res.solution = tt.set_joint_state_in_robot_state(
                coll_res.solution.joint_state, copy.deepcopy(starting_state))
            self._psi.remove_ordered_collisions(ordered_collisions)
            return coll_res
        coll_req = GetPositionIKRequest()
        coll_req.ik_request = pos_req
        coll_req.timeout = rospy.Duration(timeout)
        coll_res = self._ik_service(coll_req)
        coll_res.solution = tt.set_joint_state_in_robot_state(
            coll_res.solution.joint_state, copy.deepcopy(starting_state))
        return coll_res