Baxter逆运动学求解ik_request

忘是亡心i 2022-06-17 10:20 186阅读 0赞

输入末端姿态，手臂 “left”或“right”，即可求出每个关节所对应的角度

from baxter_core_msgs.srv import (
        SolvePositionIK,
        SolvePositionIKRequest,
    )
    from std_msgs.msg import Header
    def ik_request(self,pose,limb,verbose=True):
            self._verbose = verbose
            ns = "ExternalTools/" + limb + "/PositionKinematicsNode/IKService"
            self._iksvc = rospy.ServiceProxy(ns, SolvePositionIK)
            rospy.wait_for_service(ns, 5.0)
            hdr = Header(stamp=rospy.Time.now(), frame_id='base')
            ikreq = SolvePositionIKRequest()
            ikreq.pose_stamp.append(PoseStamped(header=hdr, pose=pose))
            #print ikreq
            try:
                resp = self._iksvc(ikreq)
            except (rospy.ServiceException, rospy.ROSException), e:
                rospy.logerr("Service call failed: %s" % (e,))
                return False
            # Check if result valid, and type of seed ultimately used to get solution
            # convert rospy's string representation of uint8[]'s to int's
            resp_seeds = struct.unpack('<%dB' % len(resp.result_type), resp.result_type)
            limb_joints = {}
            if (resp_seeds[0] != resp.RESULT_INVALID):
                seed_str = {
                            ikreq.SEED_USER: 'User Provided Seed',
                            ikreq.SEED_CURRENT: 'Current Joint Angles',
                            ikreq.SEED_NS_MAP: 'Nullspace Setpoints',
                        }.get(resp_seeds[0], 'None')
                if self._verbose:
                    print("IK Solution SUCCESS - Valid Joint Solution Found from Seed Type: {0}".format(
                            (seed_str)))
                # Format solution into Limb API-compatible dictionary
                limb_joints = dict(zip(resp.joints[0].name, resp.joints[0].position))
                if limb =='left':
                    self.left_limb_joints = limb_joints
                elif limb =='right':
                    self.right_limb_joints = limb_joints
                if self._verbose:
                    #print("IK Joint Solution:\n{0}".format(limb_joints))
                    print("------------------")
            else:
                rospy.logerr("INVALID POSE - No Valid Joint Solution Found.")
                # return False
                if limb == 'left':
                    return self.left_limb_joints
                elif limb == 'right':
                    return self.right_limb_joints
                # return limb_joints
            return limb_joints