def compute_ik()

in src/pyrobot/core.py [0:0]

• 78 lines of code
• 10 McCabe index (conditional complexity)

    def compute_ik(self, position, orientation, qinit=None, numerical=True):
        """
        Inverse kinematics

        :param position: end effector position (shape: :math:`[3,]`)
        :param orientation: end effector orientation.
                            It can be quaternion ([x,y,z,w],
                            shape: :math:`[4,]`),
                            euler angles (yaw, pitch, roll
                            angles (shape: :math:`[3,]`)),
                            or rotation matrix (shape: :math:`[3, 3]`)
        :param qinit: initial joint positions for numerical IK
        :param numerical: use numerical IK or analytical IK
        :type position: np.ndarray
        :type orientation: np.ndarray
        :type qinit: list
        :type numerical: bool
        :return: None or joint positions
        :rtype: np.ndarray
        """
        position = position.flatten()
        if orientation.size == 4:
            orientation = orientation.flatten()
            ori_x = orientation[0]
            ori_y = orientation[1]
            ori_z = orientation[2]
            ori_w = orientation[3]
        elif orientation.size == 3:
            quat = prutil.euler_to_quat(orientation)
            ori_x = quat[0]
            ori_y = quat[1]
            ori_z = quat[2]
            ori_w = quat[3]
        elif orientation.size == 9:
            quat = prutil.rot_mat_to_quat(orientation)
            ori_x = quat[0]
            ori_y = quat[1]
            ori_z = quat[2]
            ori_w = quat[3]
        else:
            raise TypeError(
                "Orientation must be in one "
                "of the following forms:"
                "rotation matrix, euler angles, or quaternion"
            )
        if qinit is None:
            qinit = self.get_joint_angles().tolist()
        elif isinstance(qinit, np.ndarray):
            qinit = qinit.flatten().tolist()
        if numerical:
            pos_tol = self.configs.ARM.IK_POSITION_TOLERANCE
            ori_tol = self.configs.ARM.IK_ORIENTATION_TOLERANCE

            req = IkCommandRequest()
            req.init_joint_positions = qinit
            req.pose = [
                position[0],
                position[1],
                position[2],
                ori_x,
                ori_y,
                ori_z,
                ori_w,
            ]
            req.tolerance = 3 * [pos_tol] + 3 * [ori_tol]

            try:
                resp = self._ik_service(req)
            except:
                rospy.logerr("IK Service call failed")
                resp = IkCommandResponse()
                resp.success = False

            if not resp.success:
                joint_positions = None
            else:
                joint_positions = resp.joint_positions
        else:
            if not hasattr(self, "ana_ik_solver"):
                raise TypeError(
                    "Analytical solver not provided, "
                    "please use `numerical=True`"
                    "to use the numerical method "
                    "for inverse kinematics"
                )
            else:
                joint_positions = self.ana_ik_solver.get_ik(
                    qinit,
                    position[0],
                    position[1],
                    position[2],
                    ori_x,
                    ori_y,
                    ori_z,
                    ori_w,
                )
        if joint_positions is None:
            return None
        print(joint_positions)
        return np.array(joint_positions)