Merge branch 'future' of github.com:petercorke/robotics-toolbox-python into future

jhavl · jhavl · commit 815e5ec53ae2 · 2023-05-01T12:26:15.000+10:00
diff --git a/roboticstoolbox/backends/PyPlot/EllipsePlot.py b/roboticstoolbox/backends/PyPlot/EllipsePlot.py
@@ -192,6 +192,9 @@ def make_ellipsoid2(self):
         #         A = np.linalg.inv(A)
         #     except:
         #         A = np.zeros((2,2))
+        if self.vell:
+            # Do the extra step for the velocity ellipse
+            A = np.linalg.inv(A)
 
         if isinstance(self.centre, str) and self.centre == "ee":
             centre = self.robot.fkine(self.q).A[:3, -1]
@@ -204,7 +207,7 @@ def make_ellipsoid2(self):
         # RVC2 p 602
         # x = sp.linalg.sqrtm(A) @ y
 
-        x, y = base.ellipse(A, inverted=True, centre=centre[:2], scale=self.scale)
+        x, y = base.ellipse(A, inverted=False, centre=centre[:2], scale=self.scale)
         self.x = x
         self.y = y
         # = x[0,:] * self.scale + centre[0]
diff --git a/roboticstoolbox/blocks/uav.py b/roboticstoolbox/blocks/uav.py
@@ -38,11 +38,18 @@ class MultiRotor(TransferBlock):
     vehicle state is a dict containing the following items:
 
         - ``x`` pose in the world frame as :math:`[x, y, z, \theta_Y, \theta_P, \theta_R]`
-        - ``vb`` translational velocity in the world frame (metres/sec)
-        - ``w`` angular rates in the world frame as yaw-pitch-roll rates (radians/second)
+        - ``trans`` position and velocity in the world frame as 
+          :math:`[x, y, z, \dot{x}, \dot{y}, \dot{z}]`
+        - ``rot`` orientation and angular rate in the world frame as 
+          :math:`[\theta_Y, \theta_P, \theta_R, \dot{\theta_Y}, \dot{\theta_P}, \dot{\theta_R}]`
+        - ``vb`` translational velocity in the body frame as
+          :math:`[\dot{x}, \dot{y}, \dot{z}]`
+        - ``w`` angular rates in the body frame as
+           :math:`[\dot{\theta_Y}, \dot{\theta_P}, \dot{\theta_R}]`
         - ``a1s`` longitudinal flapping angles (radians)
         - ``b1s`` lateral flapping angles (radians)
-
+        - ``X`` full state vector as 
+          :math:`[x, y, z, \theta_Y, \theta_P, \theta_R, \dot{x}, \dot{y}, \dot{z}, \dot{\theta_Y}, \dot{\theta_P}, \dot{\theta_R}]`
 
     The dynamic model is a dict with the following key/value pairs.
 
@@ -263,6 +270,8 @@ def output(self, t, inports, x):
         out["x"] = x[0:6]
         out["trans"] = np.r_[x[:3], vd]
         out["rot"] = np.r_[x[3:6], rpyd]
+        out["vb"] = np.linalg.inv(R) @ x[6:9]   # translational velocity mapped to body frame
+        out["w"] = iW @ x[9:12]                 # RPY rates mapped to body frame
 
         out["a1s"] = self.a1s
         out["b1s"] = self.b1s
diff --git a/roboticstoolbox/models/ETS/Planar2.py b/roboticstoolbox/models/ETS/Planar2.py
@@ -35,11 +35,11 @@ def __init__(self):
 
         super().__init__([l0, l1, l2], name="Planar2", comment="Planar 2D manipulator")
 
-        self.qr = np.array([0, 0])
+        self.qb = np.array([0, np.pi/2])
         self.qz = np.zeros(2)
 
-        self.addconfiguration("qr", self.qr)
         self.addconfiguration("qz", self.qz)
+        self.addconfiguration("qb", self.qb)
 
 
 if __name__ == "__main__":  # pragma nocover
diff --git a/roboticstoolbox/robot/DHLink.py b/roboticstoolbox/robot/DHLink.py
@@ -380,11 +380,11 @@ def __repr__(self):
         name = self.__class__.__name__
         args = []
         if self.isrevolute:
-            self._format(args, "d")
+            self._format_param(args, "d")
         else:
-            self._format(args, "theta", "θ")
-        self._format(args, "a")
-        self._format(args, "alpha", "⍺")
+            self._format_param(args, "theta", "θ")
+        self._format_param(args, "a")
+        self._format_param(args, "alpha", "⍺")
         args.extend(super()._params())
         return name + "(" + ", ".join(args) + ")"
 
diff --git a/roboticstoolbox/robot/Link.py b/roboticstoolbox/robot/Link.py
@@ -1367,61 +1367,62 @@ def format(l, fmt, val):  # noqa
 
         return dyn
 
+    def _format_param(
+        self, l, name, symbol=None, ignorevalue=None, indices=None
+    ):  # noqa  # pragma nocover
+        # if value == ignorevalue then don't display it
+
+        v = getattr(self, name)
+        s = None
+        if v is None:
+            return
+        if isinstance(v, str):
+            s = f'{name} = "{v}"'
+        elif isscalar(v) and v != ignorevalue:
+            if symbol is not None:
+                s = f"{symbol}={v:.3g}"
+            else:  # pragma: nocover
+                try:
+                    s = f"{name}={v:.3g}"
+                except TypeError:
+                    s = f"{name}={v}"
+        elif isinstance(v, np.ndarray):
+            # if np.linalg.norm(v, ord=np.inf) > 0:
+            #     if indices is not None:
+            #         flat = v.flatten()
+            #         v = np.r_[[flat[k] for k in indices]]
+            #     s = f"{name}=[" + ", ".join([f"{x:.3g}" for x in v]) + "]"
+            if indices is not None:
+                v = v.ravel()[indices]
+            s = f"{name}=" + np.array2string(
+                v,
+                separator=", ",
+                suppress_small=True,
+                formatter={"float": lambda x: f"{x:.3g}"},
+            )
+        if s is not None:
+            l.append(s)
+
     def _params(self, name: bool = True):  # pragma nocover
-        def format_param(
-            self, l, name, symbol=None, ignorevalue=None, indices=None
-        ):  # noqa  # pragma nocover
-            # if value == ignorevalue then don't display it
-
-            v = getattr(self, name)
-            s = None
-            if v is None:
-                return
-            if isinstance(v, str):
-                s = f'{name} = "{v}"'
-            elif isscalar(v) and v != ignorevalue:
-                if symbol is not None:
-                    s = f"{symbol}={v:.3g}"
-                else:  # pragma: nocover
-                    try:
-                        s = f"{name}={v:.3g}"
-                    except TypeError:
-                        s = f"{name}={v}"
-            elif isinstance(v, np.ndarray):
-                # if np.linalg.norm(v, ord=np.inf) > 0:
-                #     if indices is not None:
-                #         flat = v.flatten()
-                #         v = np.r_[[flat[k] for k in indices]]
-                #     s = f"{name}=[" + ", ".join([f"{x:.3g}" for x in v]) + "]"
-                if indices is not None:
-                    v = v.ravel()[indices]
-                s = f"{name}=" + np.array2string(
-                    v,
-                    separator=", ",
-                    suppress_small=True,
-                    formatter={"float": lambda x: f"{x:.3g}"},
-                )
-            if s is not None:
-                l.append(s)
 
         l = []  # noqa
         if name:
-            format_param(self, l, "name")
+            self._format_param(l, "name")
         if self.parent_name is not None:
             l.append('parent="' + self.parent_name + '"')
         elif isinstance(self.parent, BaseLink):
             l.append('parent="' + self.parent.name + '"')
-        format_param(self, l, "parent")
-        format_param(self, l, "isflip", ignorevalue=False)
-        format_param(self, l, "qlim")
+        self._format_param(l, "parent")
+        self._format_param(l, "isflip", ignorevalue=False)
+        self._format_param(l, "qlim")
         if self._hasdynamics:
-            format_param(self, l, "m")
-            format_param(self, l, "r")
-            format_param(self, l, "I", indices=[0, 4, 8, 1, 2, 5])
-            format_param(self, l, "Jm")
-            format_param(self, l, "B")
-            format_param(self, l, "Tc")
-            format_param(self, l, "G")
+            self._format_param(l, "m")
+            self._format_param(l, "r")
+            self._format_param(l, "I", indices=[0, 4, 8, 1, 2, 5])
+            self._format_param(l, "Jm")
+            self._format_param(l, "B")
+            self._format_param(l, "Tc")
+            self._format_param(l, "G")
 
         return l
 
diff --git a/roboticstoolbox/robot/PoERobot.py b/roboticstoolbox/robot/PoERobot.py
@@ -33,18 +33,6 @@ def __str__(self):
         s += "]"
         return s
 
-    def _repr_pretty_(self, p, cycle):
-        """
-        Pretty string for IPython (superclass method)
-
-        :param p: pretty printer handle (ignored)
-        :param cycle: pretty printer flag (ignored)
-
-        """
-        # see https://ipython.org/ipython-doc/stable/api/generated/IPython.lib.pretty.html
-
-        p.text(f"{i}:\n{str(x)}")
-
 
 class PoERevolute(PoELink):
     def __init__(self, axis, point, **kwargs):
@@ -114,26 +102,14 @@ def __str__(self):
         return s
 
     def __repr__(self):
-        s = "PoERobot([\n    "
-        for j, link in enumerate(self):
-            s += repr(link) + ","
-        s += "],\n"
+        s = "PoERobot([\n"
+        s += "\n".join(["    " + repr(link) + "," for link in self])
+        s += "\n    ],\n"
         s += f"    T0=SE3({np.array_repr(self.T0.A)}),\n"
-        s += f"    name={self.name},\n"
+        s += f"    name=\"{self.name}\",\n"
         s += ")"
         return s
 
-    def _repr_pretty_(self, p, cycle):
-        """
-        Pretty string for IPython (superclass method)
-
-        :param p: pretty printer handle (ignored)
-        :param cycle: pretty printer flag (ignored)
-
-        """
-        # see https://ipython.org/ipython-doc/stable/api/generated/IPython.lib.pretty.html
-
-        p.text(f"{i}:\n{str(x)}")
 
     def nbranches(self):
         return 0
diff --git a/tests/test_ERobot.py b/tests/test_ERobot.py
@@ -170,7 +170,7 @@ def test_teach(self):
     def test_plot_with_vellipse(self):
         robot = rtb.models.ETS.Planar2()
         e = robot.plot(
-            robot.qz, block=False, name=True, vellipse=True, limits=[1, 2, 1, 2]
+            robot.qb, block=False, name=True, vellipse=True, limits=[1, 2, 1, 2]
         )
         e.step()
         e.close()

Original file line number	Diff line number	Diff line change
`@@ -170,7 +170,7 @@ def test_teach(self):`
`170`	`170`	`def test_plot_with_vellipse(self):`
`171`	`171`	`robot = rtb.models.ETS.Planar2()`
`172`	`172`	`e = robot.plot(`
`173`		`- robot.qz, block=False, name=True, vellipse=True, limits=[1, 2, 1, 2]`
	`173`	`+ robot.qb, block=False, name=True, vellipse=True, limits=[1, 2, 1, 2]`
`174`	`174`	`)`
`175`	`175`	`e.step()`
`176`	`176`	`e.close()`