roboticstoolbox-python 1.3.0__cp313-cp313-win_amd64.whl

roboticstoolbox/models/URDF/Mico.py

@@ -0,0 +1,68 @@
+#!/usr/bin/env python
+
+import numpy as np
+from roboticstoolbox.robot.Link import Link
+from roboticstoolbox.robot.Robot import Robot
+
+
+class Mico(Robot):
+    """
+    Class that imports a Mico URDF model
+
+    ``Panda()`` is a class which imports a Kinova Mico robot definition
+    from a URDF file.  The model describes its kinematic and graphical
+    characteristics.
+
+    .. runblock:: pycon
+
+        >>> import roboticstoolbox as rtb
+        >>> robot = rtb.models.URDF.Mico()
+        >>> print(robot)
+
+    Defined joint configurations are:
+
+    - qz, zero joint angle configuration, 'L' shaped configuration
+    - qr, vertical 'READY' configuration
+
+    .. codeauthor:: Jesse Haviland
+    .. sectionauthor:: Peter Corke
+    """
+
+    def __init__(self):
+
+        links, name, urdf_string, urdf_filepath = self.URDF_read(
+            "kinova_description/urdf/j2n4s300_standalone.xacro"
+        )
+
+        gripper_base = links[6]
+
+        # Find the finger links
+        gripper_links = [link for link in links if link.parent == gripper_base]
+
+        # New intermediate link
+        gripper = Link(name="gripper", parent=gripper_base)
+        links.append(gripper)
+
+        # Set the finger link parent to be the new gripper base link
+        for g_link in gripper_links:
+            g_link._parent = gripper
+
+        super().__init__(
+            links,
+            name=name,
+            manufacturer="Kinova",
+            gripper_links=[gripper],
+            urdf_string=urdf_string,
+            urdf_filepath=urdf_filepath,
+        )
+
+        self.qr = np.array([0, 45, 60, 0]) * np.pi / 180
+        self.qz = np.zeros(4)
+
+        self.addconfiguration("qr", self.qr)
+        self.addconfiguration("qz", self.qz)
+
+
+if __name__ == "__main__":  # pragma nocover
+    robot = Mico()
+    print(robot)

roboticstoolbox/models/URDF/PR2.py

@@ -0,0 +1,64 @@
+#!/usr/bin/env python
+
+from roboticstoolbox.robot.Robot import Robot
+import numpy as np
+
+
+class PR2(Robot):
+    def __init__(self):
+
+        links, name, urdf_string, urdf_filepath = self.URDF_read(
+            "pr2_description/robots/pr2.urdf.xacro", xacro_tld="pr2_description"
+        )
+
+        super().__init__(
+            links,
+            gripper_links=[links[53], links[73]],
+            name=name,
+            urdf_string=urdf_string,
+            urdf_filepath=urdf_filepath,
+        )
+
+        self.grippers[0].tool = self.link_dict["r_gripper_tool_frame"].A()
+        self.grippers[1].tool = self.link_dict["l_gripper_tool_frame"].A()
+
+        self.manufacturer = "Willow Garage"
+
+        self.qr = np.zeros(31)
+        self.qz = np.zeros(31)
+
+        self.addconfiguration("qr", self.qr)
+        self.addconfiguration("qz", self.qz)
+
+        self.qdlim = 2.0 * np.ones(31)
+
+
+if __name__ == "__main__":  # pragma nocover
+    r = PR2()
+
+    # i = 0
+
+    # for link in r.links:
+    #     if link.isjoint:
+    #         print(i, link.name)
+
+    #         i += 1
+
+    # path, n, t = r.get_path(end=r.grippers[0])
+
+    # print(n)
+    # print(t)
+
+    # for l in path[1:]:
+    #     if len(l.collision) > 0:
+    #         print(l.isjoint)
+    #         print(l.name)
+    #         print(l.parent.name)
+    #         print()
+
+    # for l in r.grippers[0].links:
+    #     if len(l.collision) > 0:
+    #         print(l.isjoint)
+    #         print(l.name)
+    #         print(l.parent.name)
+    #         print()

roboticstoolbox/models/URDF/Panda.py

@@ -0,0 +1,67 @@
+#!/usr/bin/env python
+
+import numpy as np
+from roboticstoolbox.robot.Robot import Robot
+from spatialmath import SE3
+
+
+class Panda(Robot):
+    """
+    Class that imports a Panda URDF model
+
+    ``Panda()`` is a class which imports a Franka-Emika Panda robot definition
+    from a URDF file.  The model describes its kinematic and graphical
+    characteristics.
+
+    .. runblock:: pycon
+
+        >>> import roboticstoolbox as rtb
+        >>> robot = rtb.models.URDF.Panda()
+        >>> 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
+
+    .. codeauthor:: Jesse Haviland
+    .. sectionauthor:: Peter Corke
+    """
+
+    def __init__(self):
+
+        links, name, urdf_string, urdf_filepath = self.URDF_read(
+            "franka_description/robots/panda_arm_hand.urdf.xacro"
+        )
+
+        super().__init__(
+            links,
+            name=name,
+            manufacturer="Franka Emika",
+            gripper_links=links[9],
+            urdf_string=urdf_string,
+            urdf_filepath=urdf_filepath,
+        )
+
+        self.grippers[0].tool = SE3(0, 0, 0.1034)
+
+        self.qdlim = np.array(
+            [2.1750, 2.1750, 2.1750, 2.1750, 2.6100, 2.6100, 2.6100, 3.0, 3.0]
+        )
+
+        self.qr = np.array([0, -0.3, 0, -2.2, 0, 2.0, np.pi / 4])
+        self.qz = np.zeros(7)
+
+        self.addconfiguration("qr", self.qr)
+        self.addconfiguration("qz", self.qz)
+
+
+if __name__ == "__main__":  # pragma nocover
+    r = Panda()
+
+    r.qz
+
+    for link in r.grippers[0].links:
+        print(link)

roboticstoolbox/models/URDF/Puma560.py

@@ -0,0 +1,97 @@
+#!/usr/bin/env python
+
+import numpy as np
+from roboticstoolbox.robot.Robot import Robot
+from math import pi
+
+
+class Puma560(Robot):
+    """
+    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)
+
+    print(robot.ee_links)

roboticstoolbox/models/URDF/UR10.py

@@ -0,0 +1,53 @@
+#!/usr/bin/env python
+
+import numpy as np
+from roboticstoolbox.robot.Robot import Robot
+
+
+class UR10(Robot):
+    """
+    Class that imports a UR10 URDF model
+
+    ``UR3()`` is a class which imports a Universal Robotics UR310 robot
+    definition from a URDF file.  The model describes its kinematic and
+    graphical characteristics.
+
+    .. runblock:: pycon
+
+        >>> import roboticstoolbox as rtb
+        >>> robot = rtb.models.URDF.UR10()
+        >>> print(robot)
+
+    Defined joint configurations are:
+
+    - qz, zero joint angle configuration, 'L' shaped configuration
+    - qr, vertical 'READY' configuration
+
+    .. codeauthor:: Jesse Haviland
+    .. sectionauthor:: Peter Corke
+    """
+
+    def __init__(self):
+
+        links, name, urdf_string, urdf_filepath = self.URDF_read(
+            "ur_description/urdf/ur10_joint_limited_robot.urdf.xacro"
+        )
+
+        super().__init__(
+            links,
+            name=name.upper(),
+            manufacturer="Universal Robotics",
+            urdf_string=urdf_string,
+            urdf_filepath=urdf_filepath,
+        )
+
+        self.qr = np.array([np.pi, 0, 0, 0, np.pi / 2, 0])
+        self.qz = np.zeros(6)
+
+        self.addconfiguration("qr", self.qr)
+        self.addconfiguration("qz", self.qz)
+
+
+if __name__ == "__main__":  # pragma nocover
+    robot = UR10()
+    print(robot)

roboticstoolbox/models/URDF/UR3.py

@@ -0,0 +1,53 @@
+#!/usr/bin/env python
+
+import numpy as np
+from roboticstoolbox.robot.Robot import Robot
+
+
+class UR3(Robot):
+    """
+    Class that imports a UR3 URDF model
+
+    ``UR3()`` is a class which imports a Universal Robotics UR3 robot
+    definition from a URDF file.  The model describes its kinematic and
+    graphical characteristics.
+
+    .. runblock:: pycon
+
+        >>> import roboticstoolbox as rtb
+        >>> robot = rtb.models.URDF.UR3()
+        >>> print(robot)
+
+    Defined joint configurations are:
+
+    - qz, zero joint angle configuration, 'L' shaped configuration
+    - qr, vertical 'READY' configuration
+
+    .. codeauthor:: Jesse Haviland
+    .. sectionauthor:: Peter Corke
+    """
+
+    def __init__(self):
+
+        links, name, urdf_string, urdf_filepath = self.URDF_read(
+            "ur_description/urdf/ur3_joint_limited_robot.urdf.xacro"
+        )
+
+        super().__init__(
+            links,
+            name=name.upper(),
+            manufacturer="Universal Robotics",
+            urdf_string=urdf_string,
+            urdf_filepath=urdf_filepath,
+        )
+
+        self.qr = np.array([np.pi, 0, 0, 0, np.pi / 2, 0])
+        self.qz = np.zeros(6)
+
+        self.addconfiguration("qr", self.qr)
+        self.addconfiguration("qz", self.qz)
+
+
+if __name__ == "__main__":  # pragma nocover
+    robot = UR3()
+    print(robot)

roboticstoolbox/models/URDF/UR5.py

@@ -0,0 +1,74 @@
+#!/usr/bin/env python
+
+import numpy as np
+from roboticstoolbox.robot.Robot import Robot
+from math import pi
+
+
+class UR5(Robot):
+    """
+    Class that imports a UR5 URDF model
+
+    ``UR3()`` is a class which imports a Universal Robotics UR5 robot
+    definition from a URDF file.  The model describes its kinematic and
+    graphical characteristics.
+
+    .. runblock:: pycon
+
+        >>> import roboticstoolbox as rtb
+        >>> robot = rtb.models.URDF.UR5()
+        >>> print(robot)
+
+    Defined joint configurations are:
+
+    - qz, zero joint angle configuration, 'L' shaped configuration
+    - qr, vertical 'READY' configuration
+
+    .. codeauthor:: Jesse Haviland
+    .. sectionauthor:: Peter Corke
+    """
+
+    def __init__(self):
+
+        links, name, urdf_string, urdf_filepath = self.URDF_read(
+            "ur_description/urdf/ur5_joint_limited_robot.urdf.xacro"
+        )
+        # for link in links:
+        #     print(link)
+
+        super().__init__(
+            links,
+            name=name.upper(),
+            manufacturer="Universal Robotics",
+            gripper_links=links[7],
+            urdf_string=urdf_string,
+            urdf_filepath=urdf_filepath,
+        )
+
+        self.qr = np.array([np.pi, 0, 0, 0, np.pi / 2, 0])
+        self.qz = np.zeros(6)
+
+        self.addconfiguration("qr", self.qr)
+        self.addconfiguration("qz", self.qz)
+
+        # sol=robot.ikine_LM(SE3(0.5, -0.2, 0.2)@SE3.OA([1,0,0],[0,0,-1]))
+        self.addconfiguration_attr(
+            "qn",
+            np.array(
+                [
+                    -7.052413e-01,
+                    3.604328e-01,
+                    -1.494176e00,
+                    1.133744e00,
+                    -7.052413e-01,
+                    0,
+                ]
+            ),
+        )
+        self.addconfiguration_attr("q1", [0, -pi / 2, pi / 2, 0, pi / 2, 0])
+
+
+if __name__ == "__main__":  # pragma nocover
+    robot = UR5()
+    print(robot)
+    print(robot.ets())

roboticstoolbox/models/URDF/Valkyrie.py

@@ -0,0 +1,84 @@
+#!/usr/bin/env python
+
+import numpy as np
+from roboticstoolbox.robot.Robot import Robot, Link
+
+
+class Valkyrie(Robot):
+    """
+    Class that imports a NASA Valkyrie URDF model
+
+    ``Valkyrie()`` is a class which imports a NASA Valkyrie robot definition
+    from a URDF file.  The model describes its kinematic and graphical
+    characteristics.
+
+    .. runblock:: pycon
+
+        >>> import roboticstoolbox as rtb
+        >>> robot = rtb.models.URDF.Valkyrie()
+        >>> print(robot)
+
+    Defined joint configurations are:
+
+    - qz, zero joint angle configuration, 'L' shaped configuration
+    - qr, vertical 'READY' configuration
+
+    :reference:
+        - https://github.com/gkjohnson/nasa-urdf-robots
+
+    .. codeauthor:: Peter Corke
+    """
+
+    def __init__(self, variant="A"):
+
+        if not variant in "ABCD":
+            raise ValueError("variant must be in the range A-D")
+
+        links, name, urdf_string, urdf_filepath = self.URDF_read(
+            f"val_description/model/robots/valkyrie_{variant}.urdf"
+        )
+
+        # We wish to add an intermediate link between gripper_r_base and
+        # @gripper_r_finger_r/l
+        # This is because gripper_r_base contains a revolute joint which is
+        # a part of the core kinematic chain and not the gripper.
+        # So we wish for gripper_r_base to be part of the robot and
+        # @gripper_r_finger_r/l to be in the gripper underneath a parent Link
+        # gripper_r_base = links[13]
+        # gripper_l_base = links[33]
+
+        # # Find the finger links
+        # r_gripper_links = [link for link in links if link.parent == gripper_r_base]
+        # l_gripper_links = [link for link in links if link.parent == gripper_l_base]
+
+        # # New intermediate links
+        # r_gripper = Link(name="rightGripper", parent=gripper_r_base)
+        # l_gripper = Link(name="leftGripper", parent=gripper_l_base)
+        # links.append(r_gripper)
+        # links.append(l_gripper)
+
+        # # Set the finger link parent to be the new gripper base link
+        # for g_link in r_gripper_links:
+        #     g_link._parent = r_gripper
+
+        # for g_link in l_gripper_links:
+        #     g_link._parent = l_gripper
+
+        super().__init__(
+            links,
+            name=name,
+            manufacturer="NASA",
+            # gripper_links=[r_gripper, l_gripper],
+            urdf_string=urdf_string,
+            urdf_filepath=urdf_filepath,
+        )
+
+        # self.addconfiguration_attr("qz", np.array([0, 0, 0, 0, 0, 0, 0]))
+        # self.addconfiguration_attr("qr", np.array([0, -0.3, 0, -2.2, 0, 2.0, np.pi / 4]))
+
+
+if __name__ == "__main__":  # pragma nocover
+    robot = Valkyrie("B")
+    print(robot)
+    env = robot.plot(np.zeros((robot.n,)))
+    env.hold()

roboticstoolbox/models/URDF/YuMi.py

@@ -0,0 +1,109 @@
+#!/usr/bin/env python
+
+from roboticstoolbox.robot.Link import Link
+import numpy as np
+from roboticstoolbox.robot.Robot import Robot
+import spatialmath as sm
+
+
+class YuMi(Robot):
+    """
+    Class that imports an ABB YuMi URDF model
+
+    ``YuMi()`` is a class which imports an ABB YuMi (IRB14000) robot definition
+    from a URDF file.  The model describes its kinematic and graphical
+    characteristics.
+
+    .. runblock:: pycon
+
+        >>> import roboticstoolbox as rtb
+        >>> robot = rtb.models.URDF.YuMi()
+        >>> print(robot)
+
+    Defined joint configurations are:
+
+    - qz, zero joint angle configuration, 'L' shaped configuration
+    - qr, vertical 'READY' configuration
+
+    :reference:
+        - `https://github.com/OrebroUniversity/yumi <https://github.com/OrebroUniversity/yumi>`_
+
+    .. codeauthor:: Jesse Haviland
+    .. sectionauthor:: Peter Corke
+    """
+
+    def __init__(self):
+
+        links, name, urdf_string, urdf_filepath = self.URDF_read(
+            "yumi_description/urdf/yumi.urdf"
+        )
+
+        # We wish to add an intermediate link between gripper_r_base and
+        # @gripper_r_finger_r/l
+        # This is because gripper_r_base contains a revolute joint which is
+        # a part of the core kinematic chain and not the gripper.
+        # So we wish for gripper_r_base to be part of the robot and
+        # @gripper_r_finger_r/l to be in the gripper underneath a parent Link
+
+        gripper_r_base = links[16]
+        gripper_l_base = links[19]
+
+        # Find the finger links
+        r_gripper_links = [link for link in links if link.parent == gripper_r_base]
+        l_gripper_links = [link for link in links if link.parent == gripper_l_base]
+
+        # New intermediate links
+        r_gripper = Link(name="r_gripper", parent=gripper_l_base)
+        l_gripper = Link(name="l_gripper", parent=gripper_r_base)
+        links.append(r_gripper)
+        links.append(l_gripper)
+
+        # Set the finger link parent to be the new gripper base link
+        for g_link in r_gripper_links:
+            g_link._parent = r_gripper
+
+        for g_link in l_gripper_links:
+            g_link._parent = l_gripper
+
+        super().__init__(
+            links,
+            name=name,
+            manufacturer="ABB",
+            gripper_links=[r_gripper, l_gripper],
+            urdf_string=urdf_string,
+            urdf_filepath=urdf_filepath,
+        )
+
+        # Set the default tool transform for the end-effectors
+        self.grippers[0].tool = sm.SE3.Tz(0.13)
+        self.grippers[1].tool = sm.SE3.Tz(0.13)
+
+        self.qr = np.array(
+            [
+                0,
+                -0.3,
+                0,
+                -2.2,
+                0,
+                2.0,
+                np.pi / 4,
+                0,
+                -0.3,
+                0,
+                -2.2,
+                0,
+                2.0,
+                np.pi / 4,
+            ]
+        )
+        self.qz = np.zeros(14)
+        self.q1 = np.array([0, -0.4, 0, 0, 0, 0, 0, 0, -0.4, 0, 0, 0, 0, 0])
+
+        self.addconfiguration("qr", self.qr)
+        self.addconfiguration("qz", self.qz)
+        self.addconfiguration("q1", self.q1)
+
+
+if __name__ == "__main__":  # pragma nocover
+    robot = YuMi()
+    print(robot)