fix block tests, integrate with main tests folder

Author: petercorke

roboticstoolbox/blocks/arm.py

@@ -19,6 +19,7 @@
 """
 # The constructor of each class ``MyClass`` with a ``@block`` decorator becomes a method ``MYCLASS()`` of the BlockDiagram instance.
 
+
 # ------------------------------------------------------------------------ #
 class FKine(FunctionBlock):
     r"""
@@ -184,7 +185,7 @@ def output(self, t, inports, x):
         else:
             q0 = self.q0
 
-        sol = self.ik(inputs[0], q0=q0, seed=self.seed, **self.args)
+        sol = self.ik(inports[0], q0=q0, seed=self.seed, **self.args)
 
         if not sol.success:
             raise RuntimeError("inverse kinematic failure for pose", inports[0])
@@ -411,7 +412,6 @@ def start(self, simstate):
         )
 
     def step(self, t, inports):
-
         # update the robot plot
         self.robot.q = inports[0]
         self.env.step()
@@ -518,11 +518,12 @@ def __init__(self, q0, qf, qd0=None, qdf=None, T=None, **blockargs):
         # set T to 1 just for now
         if T is None:
             self.T = 1
-        self.start()
         self.T = T
 
-    def start(self, simstate):
+        # valid value, to allow compile to call output() before start()
+        self.start(None)
 
+    def start(self, simstate):
         if self.T is None:
             # use simulation tmax
             self.T = simstate.T
@@ -558,7 +559,6 @@ def start(self, simstate):
         )
 
     def output(self, t, inports, x):
-
         tscal = self.tscal
         ts = t / tscal
         tt = np.array([ts**5, ts**4, ts**3, ts**2, ts, 1]).T
@@ -822,7 +822,6 @@ def __init__(self, q0, qf, V=None, T=None, **blockargs):
         self.qf = qf
 
     def start(self, simstate):
-
         if self.T is None:
             self.T = simstate.T
         self.trapezoidalfunc = trapezoidal_func(self.q0, self.qf, self.T)
@@ -946,7 +945,7 @@ def start(self, simstate):
         else:
             # input based
             xmax = 1
-        sim.xmax = xmax
+        self.xmax = xmax
 
         for i in range(len(self.y0)):
             self.trajfuncs.append(trajfunc(self.y0[i], self.yf[i], xmax))
@@ -1557,7 +1556,6 @@ def deriv(self, t, inports, x):
 
 
 if __name__ == "__main__":
-
     from pathlib import Path
 
     exec(

roboticstoolbox/blocks/mobile.py

@@ -87,6 +87,7 @@ def __init__(
         #  gamma dot
 
         super().__init__(nstates=3, **blockargs)
+        self.type = "bicycle"
 
         self.vehicle = mobile.Bicycle(
             L=L, steer_max=steer_max, speed_max=speed_max, accel_max=accel_max
@@ -185,6 +186,7 @@ def __init__(
         :type blockargs: dict
         """
         super().__init__(nstates=3, **blockargs)
+        self.type = "unicycle"
 
         if x0 is None:
             self._x0 = np.zeros((self.nstates,))
@@ -299,7 +301,7 @@ def __init__(
         :type blockargs: dict
         """
         super().__init__(nstates=3, **blockargs)
-        # self.type = "diffsteer"
+        self.type = "diffsteer"
         self.R = R
 
         if x0 is None:
@@ -310,7 +312,7 @@ def __init__(
             )
             self._x0 = x0
 
-        self.vehicle = mobile.Unicycle(
+        self.vehicle = mobile.DiffSteer(
             W=w, steer_max=steer_max, speed_max=speed_max, accel_max=accel_max
         )
 

roboticstoolbox/mobile/Vehicle.py

@@ -1227,6 +1227,27 @@ def init(self):
         self._v_prev_L = [0]
         self._v_prev_R = [0]
 
+
+    def u_limited(self, u):
+        """
+        Apply vehicle velocity and acceleration limits
+
+        :param u: Desired vehicle inputs :math:`(v_L, v_R)`
+        :type u: array_like(2)
+        :return: Allowable vehicle inputs :math:`(v_L, v_R)`
+        :rtype: ndarray(2)
+
+        Velocity and acceleration limits are applied to :math:`v` and
+        turn rate limits are applied to :math:`\omega`.
+        """
+
+        # limit speed and acceleration of each wheel/track
+        ulim = np.array(u)
+        ulim[0] = self.limits_va(u[0], self._v_prev_L)
+        ulim[1] = self.limits_va(u[1], self._v_prev_R)
+
+        return ulim
+    
     def deriv(self, x, u, limits=True):
         r"""
         Time derivative of state
@@ -1270,27 +1291,8 @@ def deriv(self, x, u, limits=True):
 
         return np.r_[v * cos(theta), v * sin(theta), vdiff / self._W]
 
-    def u_limited(self, u):
-        """
-        Apply vehicle velocity and acceleration limits
-
-        :param u: Desired vehicle inputs :math:`(v_L, v_R)`
-        :type u: array_like(2)
-        :return: Allowable vehicle inputs :math:`(v_L, v_R)`
-        :rtype: ndarray(2)
-
-        Velocity and acceleration limits are applied to :math:`v` and
-        turn rate limits are applied to :math:`\omega`.
-        """
-
-        # limit speed and steer angle
-        ulim = np.array(u)
-        ulim[0] = self.limits_va(u[0], self._v_prev_L)
-        ulim[1] = self.limits_va(u[1], self._v_prev_R)
-
-        return ulim
-
 
+    
 if __name__ == "__main__":
 
     from roboticstoolbox import RandomPath