#!/usr/bin/env python

import numpy as np
from roboticstoolbox.robot.ERobot import ERobot
from math import pi


class Puma560(ERobot):
    """
    Class that imports a Puma 560 URDF model

    ``Puma560()`` is a class which imports a Unimation Puma560 robot definition
    from a URDF file.  The model describes its kinematic and graphical
    characteristics.

    .. runblock:: pycon

        >>> import roboticstoolbox as rtb
        >>> robot = rtb.models.URDF.Puma560()
        >>> print(robot)

    Defined joint configurations are:

    - qz, zero joint angle configuration, 'L' shaped configuration
    - qr, vertical 'READY' configuration
    - qs, arm is stretched out in the x-direction
    - qn, arm is at a nominal non-singular configuration

    .. warning:: This file has been modified so that the zero-angle pose is the
        same as the DH model in the toolbox. ``j3`` rotation is changed from
        -𝜋/2 to 𝜋/2.  Dimensions are also slightly different.  Both models
        include the pedestal height.

    .. note:: The original file is from https://github.com/nimasarli/puma560_description/blob/master/urdf/puma560_robot.urdf.xacro

    .. codeauthor:: Jesse Haviland
    .. sectionauthor:: Peter Corke
    """

    def __init__(self):

        links, name, urdf_string, urdf_filepath = self.URDF_read(
            "puma560_description/urdf/puma560_robot.urdf.xacro"
        )

        super().__init__(
            links,
            name=name,
            urdf_string=urdf_string,
            urdf_filepath=urdf_filepath,
        )

        self.manufacturer = "Unimation"
        # self.ee_link = self.ets[9]

        # ready pose, arm up
        self.qr = np.array([0, pi / 2, -pi / 2, 0, 0, 0])
        self.qz = np.zeros(6)

        self.addconfiguration("qr", self.qr)
        self.addconfiguration("qz", self.qz)

        # zero angles, upper arm horizontal, lower up straight up
        self.addconfiguration_attr("qz", np.array([0, 0, 0, 0, 0, 0]))

        # reference pose, arm to the right, elbow up
        self.addconfiguration_attr(
            "ru", np.array([-0.0000, 0.7854, 3.1416, -0.0000, 0.7854, 0.0000])
        )

        # reference pose, arm to the right, elbow up
        self.addconfiguration_attr(
            "rd", np.array([-0.0000, -0.8335, 0.0940, -3.1416, 0.8312, 3.1416])
        )

        # reference pose, arm to the left, elbow up
        self.addconfiguration_attr(
            "lu", np.array([2.6486, -3.9270, 0.0940, 2.5326, 0.9743, 0.3734])
        )

        # reference pose, arm to the left, elbow down
        self.addconfiguration_attr(
            "ld", np.array([2.6486, -2.3081, 3.1416, 0.6743, 0.8604, 2.6611])
        )

        # straight and horizontal
        self.addconfiguration_attr("qs", np.array([0, 0, -pi / 2, 0, 0, 0]))

        # nominal table top picking pose
        self.addconfiguration_attr("qn", np.array([0, pi / 4, pi, 0, pi / 4, 0]))


if __name__ == "__main__":  # pragma nocover

    robot = Puma560()
    print(robot)

[((57, 18, 57, 57), 'numpy.array', 'np.array', ({(57, 27, 57, 56): '[0, pi / 2, -pi / 2, 0, 0, 0]'}, {}), '([0, pi / 2, -pi / 2, 0, 0, 0])', True, 'import numpy as np\n'), ((58, 18, 58, 29), 'numpy.zeros', 'np.zeros', ({(58, 27, 58, 28): '6'}, {}), '(6)', True, 'import numpy as np\n'), ((64, 41, 64, 69), 'numpy.array', 'np.array', ({(64, 50, 64, 68): '[0, 0, 0, 0, 0, 0]'}, {}), '([0, 0, 0, 0, 0, 0])', True, 'import numpy as np\n'), ((68, 18, 68, 78), 'numpy.array', 'np.array', ({(68, 27, 68, 77): '[-0.0, 0.7854, 3.1416, -0.0, 0.7854, 0.0]'}, {}), '([-0.0, 0.7854, 3.1416, -0.0, 0.7854, 0.0])', True, 'import numpy as np\n'), ((73, 18, 73, 79), 'numpy.array', 'np.array', ({(73, 27, 73, 78): '[-0.0, -0.8335, 0.094, -3.1416, 0.8312, 3.1416]'}, {}), '([-0.0, -0.8335, 0.094, -3.1416, 0.8312, 3.1416])', True, 'import numpy as np\n'), ((78, 18, 78, 77), 'numpy.array', 'np.array', ({(78, 27, 78, 76): '[2.6486, -3.927, 0.094, 2.5326, 0.9743, 0.3734]'}, {}), '([2.6486, -3.927, 0.094, 2.5326, 0.9743, 0.3734])', True, 'import numpy as np\n'), ((83, 18, 83, 77), 'numpy.array', 'np.array', ({(83, 27, 83, 76): '[2.6486, -2.3081, 3.1416, 0.6743, 0.8604, 2.6611]'}, {}), '([2.6486, -2.3081, 3.1416, 0.6743, 0.8604, 2.6611])', True, 'import numpy as np\n'), ((87, 41, 87, 75), 'numpy.array', 'np.array', ({(87, 50, 87, 74): '[0, 0, -pi / 2, 0, 0, 0]'}, {}), '([0, 0, -pi / 2, 0, 0, 0])', True, 'import numpy as np\n'), ((90, 41, 90, 80), 'numpy.array', 'np.array', ({(90, 50, 90, 79): '[0, pi / 4, pi, 0, pi / 4, 0]'}, {}), '([0, pi / 4, pi, 0, pi / 4, 0])', True, 'import numpy as np\n')]