brieckiebot

Author: mrcyme

roboticstoolbox/examples/branched_robot.py

@@ -69,8 +69,8 @@
 
 while not arrivedl or not arrivedr:
 
-    vl, arrivedl = rtb.p_servo(la.fkine(r.q), lTep, gain=gain, threshold=0.001)
-    vr, arrivedr = rtb.p_servo(ra.fkine(r.q), rTep, gain=gain, threshold=0.001)
+    vl, arrivedl = rtb.cp_servo(la.fkine(r.q), lTep, gain=gain, threshold=0.001)
+    vr, arrivedr = rtb.cp_servo(ra.fkine(r.q), rTep, gain=gain, threshold=0.001)
 
     r.qd[la.jindices] = np.linalg.pinv(la.jacobe(r.q)) @ vl
     r.qd[ra.jindices] = np.linalg.pinv(ra.jacobe(r.q)) @ vr

roboticstoolbox/examples/puma_swift.py

@@ -1,4 +1,5 @@
 #!/usr/bin/env python
+# %%
 """
 @author John Skinner
 """

roboticstoolbox/examples/twistdemo.py

@@ -53,6 +53,10 @@ def update_twist():
 
     axis = SO3.Rx(twist_params['R']) @ SO3.Ry(twist_params['P'])
     point = [twist_params['x'], twist_params['y'], 0]
+    print(axis)
+    print(axis.A[:, 2])
+    print(point)
+    print(twist_params['p'])
     twist = Twist3.UnitRevolute(axis.A[:, 2], point, twist_params['p'])
     print(axis.A[:,2])
     update_screw_axis(axis, point)

roboticstoolbox/models/URDF/Lite6.py

@@ -31,7 +31,7 @@ class Lite6(Robot):
     """
 
     def __init__(self):
-        filepath = "ufactory_description/lite6/urdf/lite6_with_gripper_pen.urdf.xacro"
+        filepath = "ufactory_description/lite6/urdf/lite6_with_gripper.urdf.xacro"
         #filepath = "ufactory_description/lite6/urdf/lite6.urdf.xacro"
         links, name, urdf_string, urdf_filepath = self.URDF_read(
             filepath
@@ -45,7 +45,7 @@ def __init__(self):
             gripper_links=links[8],
             urdf_filepath=urdf_filepath,
         )
-        self.grippers[0].tool = SE3(0, 0, 0.15)
+        self.grippers[0].tool = SE3(0, 0, 0.8)
         self.qdlim = np.array(
             [2.1750, 2.1750, 2.1750, 2.1750, 2.6100, 2.6100, 2.6100, 3.0, 3.0]
         )

roboticstoolbox/models/URDF/Mycobot280.py

@@ -0,0 +1,68 @@
+#!/usr/bin/env python
+
+import numpy as np
+from roboticstoolbox.robot.Robot import Robot
+from spatialmath import SE3
+
+
+class Mycobot280(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.Lite6()
+        >>> 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(
+            "elephantrobotics_description/mycobot_280_pi/urdf/mycobot_280_pi.urdf"
+        )
+
+        super().__init__(
+            links,
+            name=name,
+            manufacturer="elephant_robotics",
+            #gripper_links=links[7],
+            urdf_string=urdf_string,
+            urdf_filepath=urdf_filepath,
+        )
+
+        #self.grippers[0].tool = SE3(0, 0, 0)
+
+        self.qdlim = np.array(
+            [2*np.pi, 2.61799, 5.235988, 2*np.pi, 2.1642, 2.0*np.pi]
+        )
+
+        self.qr = np.array([0, -0.3, 0, -2.2, 0, 2.0])
+        self.qz = np.zeros(6)
+
+        self.addconfiguration("qr", self.qr)
+        self.addconfiguration("qz", self.qz)
+
+"""
+if __name__ == "__main__":  # pragma nocover
+
+    r = Mycobot280()
+    r.qz
+    T = r.fkine(r.qz)
+    r.plot(r.qr, backend="swift")
+"""
+

roboticstoolbox/models/URDF/SOARM100.py

@@ -0,0 +1,67 @@
+#!/usr/bin/env python
+
+import numpy as np
+from roboticstoolbox.robot.Robot import Robot
+from spatialmath import SE3
+import roboticstoolbox as rtb
+
+class SOARM100(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):
+        filepath = "huggingface_description/SO-ARM100/urdf/SO_5DOF_ARM100_8j_URDF.SLDASM.urdf"
+        #filepath = "ufactory_description/lite6/urdf/lite6.urdf.xacro"
+        links, name, urdf_string, urdf_filepath = self.URDF_read(
+            filepath
+        )
+
+        super().__init__(
+            links,
+            name=name,
+            manufacturer="HuggingFace",
+            urdf_string=urdf_string,
+            gripper_links=links[6],
+            urdf_filepath=urdf_filepath,
+        )
+        #self.grippers[0].tool = SE3(0, 0, 0.4)
+        self.qdlim = np.array(
+            [2.1750, 2.1750, 2.1750, 2.1750, 2.6100]
+        )
+
+        self.qr = np.array([0, -0.6, 1, 0, 1])
+        self.qz = np.zeros(5)
+
+        self.addconfiguration("qr", self.qr)
+        self.addconfiguration("qz", self.qz)
+
+
+if __name__ == "__main__":  # pragma nocover
+
+    r = SOARM100()
+    r.qz
+    T = r.fkine(r.qz)
+    r.plot(r.qr, backend="swift")
+
+

roboticstoolbox/models/URDF/__init__.py

@@ -24,6 +24,8 @@
 from roboticstoolbox.models.URDF.Valkyrie import Valkyrie
 from roboticstoolbox.models.URDF.AL5D import AL5D
 from roboticstoolbox.models.URDF.Lite6 import Lite6
+from roboticstoolbox.models.URDF.SOARM100 import SOARM100
+from roboticstoolbox.models.URDF.Mycobot280 import Mycobot280
 
 __all__ = [
     "Panda",
@@ -51,5 +53,7 @@
     "FetchCamera",
     "Valkyrie",
     "AL5D",
-    "Lite6"
+    "Lite6", 
+    "Mycobot280",
+    "SOARM100",
 ]

roboticstoolbox/robot/IK.py

@@ -488,13 +488,11 @@ def _check_jl(self, ets: "rtb.ETS", q: np.ndarray) -> bool:
         True if joints within feasible limits otherwise False
 
         """
-
         # Loop through the joints in the ETS
         for i in range(ets.n):
             # Get the corresponding joint limits
             ql0 = ets.qlim[0, i]
             ql1 = ets.qlim[1, i]
-
             # Check if q exceeds the limits
             if q[i] < ql0 or q[i] > ql1:
                 return False