roboticstoolbox-python 1.3.0__cp312-cp312-win_amd64.whl

roboticstoolbox/backends/Dynamixel/dynamixel_sdk/robotis_def.py

@@ -0,0 +1,75 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+
+################################################################################
+# Copyright 2017 ROBOTIS CO., LTD.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+################################################################################
+
+# Author: Ryu Woon Jung (Leon)
+
+BROADCAST_ID = 0xFE  # 254
+MAX_ID = 0xFC  # 252
+
+# Instruction for DXL Protocol
+INST_PING = 1
+INST_READ = 2
+INST_WRITE = 3
+INST_REG_WRITE = 4
+INST_ACTION = 5
+INST_FACTORY_RESET = 6
+INST_CLEAR = 16
+INST_SYNC_WRITE = 131  # 0x83
+INST_BULK_READ = 146  # 0x92
+# --- Only for 2.0 ---
+INST_REBOOT = 8
+INST_STATUS = 85  # 0x55
+INST_SYNC_READ = 130  # 0x82
+INST_BULK_WRITE = 147  # 0x93
+
+# Communication Result
+COMM_SUCCESS = 0  # tx or rx packet communication success
+COMM_PORT_BUSY = -1000  # Port is busy (in use)
+COMM_TX_FAIL = -1001  # Failed transmit instruction packet
+COMM_RX_FAIL = -1002  # Failed get status packet
+COMM_TX_ERROR = -2000  # Incorrect instruction packet
+COMM_RX_WAITING = -3000  # Now recieving status packet
+COMM_RX_TIMEOUT = -3001  # There is no status packet
+COMM_RX_CORRUPT = -3002  # Incorrect status packet
+COMM_NOT_AVAILABLE = -9000  #
+
+
+# Macro for Control Table Value
+def DXL_MAKEWORD(a, b):
+    return (a & 0xFF) | ((b & 0xFF) << 8)
+
+
+def DXL_MAKEDWORD(a, b):
+    return (a & 0xFFFF) | (b & 0xFFFF) << 16
+
+
+def DXL_LOWORD(l):
+    return l & 0xFFFF
+
+
+def DXL_HIWORD(l):
+    return (l >> 16) & 0xFFFF
+
+
+def DXL_LOBYTE(w):
+    return w & 0xFF
+
+
+def DXL_HIBYTE(w):
+    return (w >> 8) & 0xFF

roboticstoolbox/backends/Dynamixel/dyndata.py

@@ -0,0 +1,121 @@
+from bs4 import BeautifulSoup
+import urllib.request
+import re
+from collections import namedtuple
+import json
+
+url = "https://emanual.robotis.com/docs/en/software/dynamixel/dynamixel_workbench/"
+
+
+def table1_element(tables, name):
+    try:
+        value = (
+            tables[0].find(string=re.compile(name)).parent.find_next_siblings("td")[0]
+        )
+        print(name, value.text)
+        return value.text
+    except Exception:
+        print(name, " not found")
+        return None
+
+
+def table2_element(tables, name, col=2):
+    try:
+        value = (
+            tables[1]
+            .find(string=re.compile(name))
+            .parent.parent.find_next_siblings("td")[col]
+        )
+        print(name, value.text)
+        return value.text
+    except Exception:
+        print(name, " not found")
+        return None
+
+
+def getint(s):
+    print("getint: ", s)
+    r = re.compile("[0-9,]+")
+    m = r.search(s)
+    if m is None:
+        return None
+    else:
+        return int(m.group(0).replace(",", ""))
+
+
+def getfloat(s):
+    r = re.compile("[0-9,\.]+")
+    m = r.search(s)
+    return float(m.group(0).replace(",", ""))
+
+
+with urllib.request.urlopen(url) as response:
+    html = response.read()
+
+    soup = BeautifulSoup(html, features="lxml")
+
+    h = soup.find_all("h1", id="supported-dynamixel")
+    table = h[0].find_next_sibling("table")
+    models = []
+    for tr in table.find_all("tr"):
+        for a in tr.find_all("a"):
+            print(a.text, "https://emanual.robotis.com" + a["href"])
+            models.append((a.text, "https://emanual.robotis.com" + a["href"]))
+
+dyndata = namedtuple("dynamixel_data", "model res posmin posmax")
+dyndict = {}
+for model, url in models:
+    print("MODEL", model)
+    with urllib.request.urlopen(url) as response:
+        motor = {}
+        motor["model"] = model
+        motor["url"] = url
+
+        html = response.read()
+
+        soup = BeautifulSoup(html, features="lxml")
+        tables = soup.find_all("table")
+
+        # first table, find resolution
+        res = table1_element(tables, "Resolution.*")
+        r = res.split(" ")
+        r[0] = getfloat(r[0])
+        motor["resolution"] = r
+
+        rd = table1_element(tables, "Running Degree.*")
+        if rd is not None:
+            # 0 [deg] ~ 300 [deg]
+            rd = rd.split("~")
+            motor["angular_range"] = [getint(x) for x in rd]
+
+        # second table, find min/max position, model
+        col = 2
+        model = table2_element(tables, "Model.*")
+        if model == "-":
+            # some series (eg. X) have one less column in this table
+            col = 1
+            model = table2_element(tables, "Model.*", col)
+
+        model = getint(model)
+        motor["modelnum"] = model
+
+        cw = table2_element(tables, "CW Angle Limit.*", col)
+        if cw is not None:
+            cw = getint(cw)
+            ccw = table2_element(tables, "CCW Angle Limit.*", col)
+            ccw = getint(ccw)
+            motor["encoder_range"] = (cw, ccw)
+
+        max = table2_element(tables[1], "Max.* Pos.*", col)
+        if max is not None:
+            max = int(max.text.replace(",", ""))
+
+            min = table2_element(tables[1], "Min.* Pos.*", col)
+            min = int(min.text.replace(",", ""))
+            motor["encoder_range"] = (min, max)
+
+        dyndict[model] = motor
+
+print(dyndict)
+with open("dynamixel.json", "w") as outfile:
+    json.dump(dyndict, outfile, indent=4)

roboticstoolbox/backends/PyPlot/EllipsePlot.py

@@ -0,0 +1,253 @@
+#!/usr/bin/env python
+"""
+@author Jesse Haviland
+"""
+
+import numpy as np
+import warnings
+
+# import scipy as sp
+from spatialmath import base
+import matplotlib.pyplot as plt
+
+
+class ShapePlot:
+    def __init__(self, shape, wireframe=True, **kwargs):
+        self.shape = shape  # reference to the spatialgeom shape
+        self.wireframe = wireframe
+        self.args = kwargs
+        self.mpl = None
+
+    def plot(self):
+        if ax is None:
+            ax = self.ax
+
+        if ax is None:
+            plt.figure()
+            ax = plt.axes(projection="3d")
+            self.ax = ax
+
+        self.draw()
+
+    def draw(self, ax=None):
+        # TODO only remove and redraw if base has changed
+        if self.mpl is not None:
+            self.mpl.remove()
+
+        if self.shape.stype == "cuboid":
+            # scale
+            self.mpl = base.plot_cuboid(
+                sides=self.shape.scale, pose=self.shape.base, ax=ax
+            )
+
+        elif self.shape.stype == "sphere":
+            print(self.shape.base.t)
+            self.mpl = base.plot_sphere(self.shape.radius, pose=self.shape.base, ax=ax)
+
+        elif self.shape.stype == "cylinder":
+            # radius, length
+            pass
+
+    def make(self):
+        pass
+
+
+class EllipsePlot:
+    def __init__(self, robot, q, etype, opt="trans", centre=[0, 0, 0], scale=1.0):
+        super(EllipsePlot, self).__init__()
+
+        try:
+            centre = base.getvector(centre, 3)
+        except ValueError:
+            centre = base.getvector(centre, 2)
+            centre = np.array([centre[0], centre[1], 0])
+        except TypeError:
+            if centre != "ee":
+                raise ValueError(
+                    "Centre must be a three vector or 'ee' meaningend-effector"
+                )
+
+        self.ell = None
+        self.robot = robot
+        self.opt = str(opt).lower()
+        self.centre = centre
+        self.ax = None
+        self.scale = scale
+        self._valid_opt = self.opt in ("trans", "rot")
+
+        if not self._valid_opt:
+            warnings.warn(
+                f"Invalid ellipse opt={opt!r}. Expected 'trans' or 'rot'; ellipse will not be drawn.",
+                RuntimeWarning,
+                stacklevel=2,
+            )
+
+        if q is None:
+            self.q = robot.q
+        else:
+            self.q = q
+
+        if etype == "v":
+            self.vell = True
+            self.name = "Velocity Ellipse"
+        elif etype == "f":
+            self.vell = False
+            self.name = "Force Ellipse"
+
+    def draw(self):
+        ok = self.make_ellipsoid()
+        if not ok:
+            return
+
+        if self.ell is not None:
+            self.ell.remove()
+            # print(type(self.ell))
+            # print(type(self.ax))
+            # assert True == False
+            # self.ax.collections.remove(self.ell)
+
+        self.ell = self.ax.plot_wireframe(
+            self.x, self.y, self.z, rstride=6, cstride=6, color="#2980b9", alpha=0.2
+        )
+
+    def draw2(self):
+        ok = self.make_ellipsoid2()
+        if not ok:
+            return
+
+        if self.ell is not None:
+            self.ell[0].set_data(self.x, self.y)
+        else:
+            self.ell = self.ax.plot(self.x, self.y, color="#2980b9")
+
+    def plot(self, ax=None):
+        if ax is None:
+            ax = self.ax
+
+        if ax is None:
+            plt.figure()
+            ax = plt.axes(projection="3d")
+            self.ax = ax
+
+        self.draw()
+
+    def plot2(self, ax=None):
+        if ax is None:
+            ax = self.ax
+
+        if ax is None:
+            ax = plt.axes()
+            self.ax = ax
+
+        self.draw2()
+
+    def make_ellipsoid(self):
+        """
+        Plot the 3d Ellipsoid
+
+        """
+
+        if not self._valid_opt:
+            return False
+
+        if self.opt == "trans":
+            J = self.robot.jacobe(self.q)[:3, :]
+            A = J @ J.T
+        elif self.opt == "rot":
+            J = self.robot.jacobe(self.q)[3:, :]
+            A = J @ J.T
+
+        if not self.vell:
+            # Do the extra step for the force ellipse
+            try:
+                A = np.linalg.inv(A)
+            except np.linalg.LinAlgError:
+                warnings.warn(
+                    "Force ellipsoid is singular at this configuration; skipping ellipse draw.",
+                    RuntimeWarning,
+                    stacklevel=2,
+                )
+                return False
+
+        if isinstance(self.centre, str) and self.centre == "ee":
+            centre = self.robot.fkine(self.q).t
+        else:
+            centre = self.centre
+
+        # find the rotation matrix and radii of the axes
+        U, s, rotation = np.linalg.svd(A)
+        if np.any(s <= 0):
+            warnings.warn(
+                "Ellipsoid matrix is not positive definite; skipping ellipse draw.",
+                RuntimeWarning,
+                stacklevel=2,
+            )
+            return False
+        radii = 1.0 / np.sqrt(s)
+
+        # points on unit sphere
+        u = np.linspace(0.0, 2.0 * np.pi, 50)
+        v = np.linspace(0.0, np.pi, 50)
+        x = radii[0] * np.outer(np.cos(u), np.sin(v))
+        y = radii[1] * np.outer(np.sin(u), np.sin(v))
+        z = radii[2] * np.outer(np.ones_like(u), np.cos(v))
+
+        # transform points to ellipsoid
+        for i in range(len(x)):
+            for j in range(len(x)):
+                [x[i, j], y[i, j], z[i, j]] = np.dot(
+                    [x[i, j], y[i, j], z[i, j]], rotation
+                )
+
+        self.x = x * self.scale + centre[0]
+        self.y = y * self.scale + centre[1]
+        self.z = z * self.scale + centre[2]
+        return True
+
+    def make_ellipsoid2(self):
+        """
+        Plot the 2d Ellipsoid
+
+        """
+
+        if not self._valid_opt:
+            return False
+
+        if self.opt == "trans":
+            J = self.robot.jacob0(self.q)[:2, :]
+            A = J @ J.T
+        elif self.opt == "rot":
+            warnings.warn(
+                "Cannot draw rotational ellipse for a 2D robot plot; skipping ellipse draw.",
+                RuntimeWarning,
+                stacklevel=2,
+            )
+            return False
+
+        # velocity ellipsoid is E = A^-1
+        # force ellipsoid is E = A
+        #
+        # rather than compute the inverse we can have base.ellipse()  do it for us
+        # using the inverted argument. For the velocity ellipse we already have the
+        # inverse of E, it is A, so we set inverted=True.
+
+        if isinstance(self.centre, str) and self.centre == "ee":
+            centre = self.robot.fkine(self.q).A[:3, -1]
+        else:
+            centre = self.centre
+
+        try:
+            x, y = base.ellipse(
+                A, inverted=self.vell, centre=centre[:2], scale=self.scale
+            )
+        except np.linalg.LinAlgError:
+            warnings.warn(
+                "Ellipse is singular at this configuration; skipping ellipse draw.",
+                RuntimeWarning,
+                stacklevel=2,
+            )
+            return False
+
+        self.x = x
+        self.y = y
+        return True