roboticstoolbox-python 1.4.0__py3-none-any.whl

roboticstoolbox/models/URDF/Fetch.py

@@ -0,0 +1,96 @@
+#!/usr/bin/env python
+
+import re
+
+import numpy as np
+from roboticstoolbox.models.URDF.URDFRobot import URDFRobot
+
+# from spatialmath import SE3
+
+
+def _patch_fetch_urdf(text: str) -> str:
+    """Work around a broken upstream file served by robot_descriptions.
+
+    Applies to: robot_descriptions v2.0.0's ``fetch_description``, which
+    clones ``openai/roboschool`` at commit ``c8ee2812`` and exposes
+    ``roboschool/models_robot/fetch_description/robots/fetch.urdf``.
+
+    That file is not well-formed XML: it has a single
+    ``<sensor:camera>...</sensor:camera>`` block (inside a trailing
+    ``<gazebo reference="head_camera_rgb_optical_frame">`` element) using the
+    ``sensor:`` namespace prefix, but no ``xmlns:sensor`` is ever declared
+    anywhere in the document — old pre-SDF Gazebo camera-sensor syntax
+    (circa ROS Fuerte/Groovy) that was never valid XML to begin with.
+    ``roboschool`` is an archived (read-only) GitHub repo, so this can't be
+    fixed upstream. The block is pure Gazebo simulation config (image
+    format/size/FOV/clip planes for a camera plugin) with no bearing on
+    kinematics, dynamics, or geometry, so it's safe to drop entirely.
+
+    If a future robot_descriptions update points at a fixed fork or file,
+    this patch becomes a no-op (the regex simply won't match) — safe to
+    remove once confirmed unnecessary.
+    """
+    return re.sub(
+        r"\s*<gazebo reference=\"head_camera_rgb_optical_frame\">\s*"
+        r"<sensor:camera.*?</sensor:camera>\s*</gazebo>\n?",
+        "\n",
+        text,
+        flags=re.DOTALL,
+    )
+
+
+class Fetch(URDFRobot):
+    """
+    Class that imports a Fetch URDF model
+
+    ``Fetch()`` is a class which imports a Fetch robot definition
+    from a URDF file.  The model describes its kinematic and graphical
+    characteristics.
+
+    .. runblock:: pycon
+
+        >>> import roboticstoolbox as rtb
+        >>> robot = rtb.models.URDF.Fetch()
+        >>> print(robot)
+
+    Defined joint configurations are:
+
+    - qz, zero joint angle configuration, arm is stretched out in the x-direction
+    - qr, tucked arm configuration
+
+    .. codeauthor:: Kerry He
+    .. sectionauthor:: Peter Corke
+    """
+
+    def __init__(self):
+        super().__init__(
+            "fetch",
+            manufacturer="Fetch",
+            gripper_link_index=11,
+            patch=_patch_fetch_urdf,
+        )
+
+        self.qdlim = np.array([4.0, 4.0, 0.1, 1.25, 1.45, 1.57, 1.52, 1.57, 2.26, 2.26])
+
+        self.qz = np.array([0, 0, 0, 0, 0, 0, 0, 0, 0, 0])
+        self.qr = np.array([0, 0, 0.05, 1.32, 1.4, -0.2, 1.72, 0, 1.66, 0])
+
+        self.addconfiguration("qr", self.qr)
+        self.addconfiguration("qz", self.qz)
+
+
+if __name__ == "__main__":  # pragma nocover
+    robot = Fetch()
+    print(robot)
+
+    for link in robot.links:
+        print(link.name)
+        print(link.isjoint)
+        print(len(link.collision))
+
+    print()
+
+    for link in robot.grippers[0].links:
+        print(link.name)
+        print(link.isjoint)
+        print(len(link.collision))

roboticstoolbox/models/URDF/Frankie.py

@@ -0,0 +1,67 @@
+#!/usr/bin/env python
+
+import numpy as np
+from roboticstoolbox.models.URDF.URDFRobot import URDFRobot
+from spatialmath import SE3
+
+
+class Frankie(URDFRobot):
+    """
+    Class that imports a Frankie URDF model
+
+    ``Frankie()`` is a class which imports a Frankie robot definition
+    from a URDF file.  The model describes its kinematic and graphical
+    characteristics.
+
+    .. runblock:: pycon
+
+        >>> import roboticstoolbox as rtb
+        >>> robot = rtb.models.URDF.Frankie()
+        >>> 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):
+        super().__init__(
+            "panda",
+            manufacturer="Franka Emika",
+            gripper_link_index=9,
+        )
+
+        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
+    robot = Frankie()
+    print(robot)
+
+    for link in robot.links:
+        print(link.name)
+        print(link.isjoint)
+        print(len(link.collision))
+
+    print()
+
+    for link in robot.grippers[0].links:
+        print(link.name)
+        print(link.isjoint)
+        print(len(link.collision))

roboticstoolbox/models/URDF/FrankieOmni.py

@@ -0,0 +1,94 @@
+#!/usr/bin/env python
+
+import numpy as np
+from roboticstoolbox.robot.Robot import Robot
+from roboticstoolbox.robot.Link import Link
+from roboticstoolbox.ets.ETS import ETS
+from roboticstoolbox.ets.ET import ET
+from roboticstoolbox.models.URDF.URDFRobot import URDF_read
+from spatialmath import SE3
+
+
+class FrankieOmni(Robot):
+    """
+    Class that imports an Omnidirectional Frankie URDF model
+
+    ``FrankieOmni()`` is a class which imports a FrankieOmni robot definition
+    from a URDF file.  The model describes its kinematic and graphical
+    characteristics.
+
+    .. runblock:: pycon
+
+        >>> import roboticstoolbox as rtb
+        >>> robot = rtb.models.URDF.FrankieOmni()
+        >>> 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_base, _, urdf_filepath = URDF_read(
+            "clearpath_ridgeback_description/urdf/ridgeback.urdf.xacro"
+        )
+
+        links_panda, _, _ = URDF_read(
+            "qut_frankie_description/robots/panda_arm_hand.urdf.xacro"
+        )
+
+        base_link = links_base[9]
+        base_arm = Link(ETS(ET.tz(0.28)), name="base_arm", parent=base_link)
+
+        links_panda[0]._parent = base_arm
+        links_all = links_base + links_panda
+        links_all.append(base_arm)
+
+        super().__init__(
+            links_all,
+            name="FrankieOmni",
+            manufacturer="Custom",
+            gripper_links=links_all[28],
+        )
+        self._urdf_filepath = str(urdf_filepath) if urdf_filepath is not None else ""
+
+        self.grippers[0].tool = SE3(0, 0, 0.1034)
+
+        self.qdlim = np.array(
+            [
+                4.0,
+                4.0,
+                4.0,
+                2.1750,
+                2.1750,
+                2.1750,
+                2.1750,
+                2.6100,
+                2.6100,
+                2.6100,
+                3.0,
+                3.0,
+            ]
+        )
+
+        self.qr = np.array([0, 0, 0, 0, -0.3, 0, -2.2, 0, 2.0, np.pi / 4])
+        self.qz = np.zeros(10)
+
+        self.addconfiguration("qr", self.qr)
+        self.addconfiguration("qz", self.qz)
+
+
+if __name__ == "__main__":  # pragma nocover
+    pass
+
+    # r = Panda()
+
+    # for link in r.grippers[0].links:
+    #     print(link)

roboticstoolbox/models/URDF/Jaco.py

@@ -0,0 +1,49 @@
+#!/usr/bin/env python
+
+import numpy as np
+from roboticstoolbox.models.URDF.URDFRobot import URDFRobot
+
+
+class Jaco(URDFRobot):
+    """
+    Class that imports a Kinova Jaco URDF model
+
+    ``Jaco()`` is a class which imports a Kinova Jaco 2 (j2n6s200) robot definition
+    from a URDF file.  The model describes its kinematic and graphical
+    characteristics.
+
+    .. runblock:: pycon
+
+        >>> import roboticstoolbox as rtb
+        >>> robot = rtb.models.URDF.Jaco()
+        >>> print(robot)
+
+    Defined joint configurations are:
+
+    - qz, zero joint angle configuration
+    - qr, nominal 'ready' configuration
+
+    .. codeauthor:: Peter Corke
+    .. sectionauthor:: Peter Corke
+    """
+
+    def __init__(self):
+
+        super().__init__(
+            "j2n6s200",
+            manufacturer="Kinova",
+            gripper_link_index=9,
+        )
+
+        # 6 arm joints + 4 gripper joints (2 fingers x [finger, finger_tip]),
+        # gripper joints left at 0 (fully open) for both named configurations
+        self.qr = np.array([0, 45, 60, 0, 0, 0, 0, 0, 0, 0]) * np.pi / 180
+        self.qz = np.zeros(10)
+
+        self.addconfiguration("qr", self.qr)
+        self.addconfiguration("qz", self.qz)
+
+
+if __name__ == "__main__":  # pragma nocover
+    robot = Jaco()
+    print(robot)

roboticstoolbox/models/URDF/KinovaGen3.py

@@ -0,0 +1,63 @@
+#!/usr/bin/env python
+
+import numpy as np
+from roboticstoolbox.models.URDF.URDFRobot import URDFRobot
+
+
+class KinovaGen3(URDFRobot):
+    """
+    Class that imports a KinovaGen3 URDF model
+
+    ``KinovaGen3()`` is a class which imports a KinovaGen3 robot definition
+    from a URDF file.  The model describes its kinematic and graphical
+    characteristics.
+
+    .. runblock:: pycon
+
+        >>> import roboticstoolbox as rtb
+        >>> robot = rtb.models.URDF.KinovaGen3()
+        >>> 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):
+        super().__init__(
+            "gen3",
+            manufacturer="Kinova",
+            gripper_link_index=10,
+        )
+
+        # 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.deg2rad([0.0, 15.0, 180.0, 230.0, 0.0, 55.0, 90.0])
+        self.qz = np.zeros(7)
+
+        self.addconfiguration("qr", self.qr)
+        self.addconfiguration("qz", self.qz)
+
+
+if __name__ == "__main__":  # pragma nocover
+    robot = KinovaGen3()
+    robot.q = robot.qr
+    print(robot)
+
+    from swift import Swift
+
+    env = Swift()
+    env.launch()
+    env.add(robot)
+    env.hold()
+
+    for link in robot.links:
+        print("\n")
+        print(link.isjoint)

roboticstoolbox/models/URDF/LBR.py

@@ -0,0 +1,57 @@
+#!/usr/bin/env python
+
+import numpy as np
+from roboticstoolbox.models.URDF.URDFRobot import URDFRobot
+
+
+class LBR(URDFRobot):
+    """
+    Class that imports a Kuka LBR iiwa URDF model
+
+    ``LBR()`` is a class which imports a Kuka LBR iiwa 14 R820 robot definition
+    from a URDF file.  The robot has a payload of 14 kg, and a reach of 820 mm.
+
+    The model describes its kinematic and graphical characteristics.
+
+    .. runblock:: pycon
+
+        >>> import roboticstoolbox as rtb
+        >>> robot = rtb.models.URDF.LBR()
+        >>> 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):
+
+        # The bundled file does $(find kuka_lbr_iiwa_support) to locate its
+        # own macro file, but rtb-data ships that content under the
+        # directory name "kuka_lbr_iiwa" (no "_support" suffix), so xacro's
+        # package lookup can't find it without this alias.
+        super().__init__(
+            "kuka_description/kuka_lbr_iiwa/urdf/lbr_iiwa_14_r820.xacro",
+            manufacturer="Kuka",
+            extra_packages={"kuka_lbr_iiwa_support": "kuka_description/kuka_lbr_iiwa"},
+        )
+
+        # 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, -1.9, 0, 1.5, np.pi / 4])
+        self.qz = np.zeros(7)
+
+        self.addconfiguration("qr", self.qr)
+        self.addconfiguration("qz", self.qz)
+
+
+if __name__ == "__main__":  # pragma nocover
+    robot = LBR()
+    print(robot)

roboticstoolbox/models/URDF/PR2.py

@@ -0,0 +1,66 @@
+#!/usr/bin/env python
+
+import numpy as np
+from roboticstoolbox.models.URDF.URDFRobot import URDFRobot
+
+
+class PR2(URDFRobot):
+    """
+    Class that imports a PR2 URDF model
+
+    ``PR2()`` is a class which imports a Willow Garage PR2 robot definition
+    from a URDF file.  The model describes its kinematic and graphical
+    characteristics.
+
+    .. runblock:: pycon
+
+        >>> import roboticstoolbox as rtb
+        >>> robot = rtb.models.URDF.PR2()
+        >>> print(robot)
+
+    .. codeauthor:: Jesse Haviland
+    .. sectionauthor:: Peter Corke
+    """
+
+    def __init__(self):
+
+        super().__init__("pr2", manufacturer="Willow Garage")
+
+        self.qr = np.zeros(self.n)
+        self.qz = np.zeros(self.n)
+
+        self.addconfiguration("qr", self.qr)
+        self.addconfiguration("qz", self.qz)
+
+        self.qdlim = 2.0 * np.ones(self.n)
+
+
+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,60 @@
+#!/usr/bin/env python
+
+import numpy as np
+from roboticstoolbox.models.URDF.URDFRobot import URDFRobot
+from spatialmath import SE3
+
+
+class Panda(URDFRobot):
+    """
+    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):
+
+        super().__init__(
+            "qut_frankie_description/robots/panda_arm_hand.urdf.xacro",
+            manufacturer="Franka Emika",
+            gripper_link_index=9,
+        )
+
+        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,90 @@
+#!/usr/bin/env python
+
+import numpy as np
+from roboticstoolbox.models.URDF.URDFRobot import URDFRobot
+from math import pi
+
+
+class Puma560(URDFRobot):
+    """
+    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):
+
+        super().__init__(
+            "unimation_puma560_description/urdf/puma560_robot.urdf.xacro",
+            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,43 @@
+#!/usr/bin/env python
+
+import numpy as np
+from roboticstoolbox.models.URDF.URDFRobot import URDFRobot
+
+
+class UR10(URDFRobot):
+    """
+    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):
+
+        super().__init__("ur10", manufacturer="Universal Robotics")
+
+        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)