moteus 0.3.71__tar.gz → 0.3.72__tar.gz

{moteus-0.3.71 → moteus-0.3.72}/PKG-INFO

@@ -1,6 +1,6 @@
 Metadata-Version: 2.1
 Name: moteus
-Version: 0.3.71
+Version: 0.3.72
 Summary: moteus brushless controller library and tools
 Home-page: https://github.com/mjbots/moteus
 Author: mjbots Robotic Systems
@@ -15,8 +15,10 @@ Description-Content-Type: text/markdown
 Requires-Dist: pyserial>=3.5
 Requires-Dist: python-can>=3.3
 Requires-Dist: pyelftools>=0.26
+Requires-Dist: scipy>=1.8.0
 Requires-Dist: importlib_metadata>=3.6
 Requires-Dist: pywin32; platform_system == "Windows"
+Requires-Dist: numpy<2
 
 # Python bindings for moteus brushless controller #
 

{moteus-0.3.71 → moteus-0.3.72}/moteus/calibrate_encoder.py

@@ -14,6 +14,9 @@
 
 import json
 import math
+import scipy.optimize
+
+BIN_COUNT = 64
 
 class Entry:
     direction = 0
@@ -159,8 +162,8 @@ def _window_average(values, window_size):
         end = i + window_size // 2
         errs = [0] * (end - start)
         for j in range(start, end):
-            errs[j - start] = _wrap_neg_pi_to_pi(values[wrap(j)] - values[wrap(start)])
-        result[i] = values[wrap(start)] + (sum(errs) / len(errs))
+            errs[j - start] = values[wrap(j)]
+        result[i] = sum(errs) / len(errs)
 
     return result
 
@@ -176,6 +179,8 @@ class CalibrationResult:
         self.total_delta = None
         self.ratio = None
 
+        self.fit_metric = None
+
         self.errors = []
 
     def __repr__(self):
@@ -184,6 +189,7 @@ class CalibrationResult:
             "phase_invert": self.phase_invert,
             "poles": self.poles,
             "offset": self.offset,
+            "fit_metric": self.fit_metric,
             "errors": self.errors,
             })
 
@@ -193,13 +199,16 @@ class CalibrationResult:
             'phase_invert': self.phase_invert,
             'poles': self.poles,
             'offset': self.offset,
+            'fit_metric': self.fit_metric,
         }
 
 
 def calibrate(parsed,
               desired_direction=1,
               max_remainder_error=0.1,
-              allow_phase_invert=True):
+              allow_phase_invert=True,
+              allow_optimize=True,
+              force_optimize=False):
     '''Calibrate the motor.
 
     :param desired_direction: For positive unwrapped_position, should
@@ -315,19 +324,89 @@ def calibrate(parsed,
 
     expected = [(2.0 * math.pi / 65536.0) * (result.poles / 2) * x for x in xpos]
 
-    err = [_wrap_neg_pi_to_pi(a - b) for a, b in zip(avg_interp, expected)]
+    err = [a - b for a, b in zip(avg_interp, expected)]
 
-    # Make the error seem reasonable, so unwrap if we happen to span
-    # the pi boundary.
-    if (max(err) - min(err)) > 1.5 * math.pi:
-        err = [x if x > 0 else x + 2 * math.pi for x in err]
+    # Make the error balanced about 0.
+    mean_err = sum(err) / len(err)
+    wrapped_mean_err = _wrap_neg_pi_to_pi(mean_err)
+    delta_mean_err = mean_err - wrapped_mean_err
+    err = [x - delta_mean_err for x in err]
 
     avg_window = int(len(err) / result.poles)
     avg_err = _window_average(err, avg_window)
 
-    offset_x = list(range(0, 65536, 1024))
+    offset_x = list(range(0, 65536, 65536 // BIN_COUNT))
     offset = _interpolate(offset_x, xpos, avg_err)
 
+    MAX_ERROR = 0.8
+
+    # The firmware will complain if individual steps are more
+    # than this apart.
+    MAX_STEP_CHANGE = 3.5
+
+    # Penalize solutions that have error greater than this.
+    PENALTY_RATIO = 0.80
+
+    def full_metric(x, *args):
+        errors = 0
+        result = 0
+        resampled = _interpolate(xpos, offset_x + [65536],
+                                 list(x) + [x[0]])
+        for a, b in zip(err, resampled):
+            this_err = abs(_wrap_neg_pi_to_pi(a - b))
+            # Heavily penalize if we would have an error.
+            if this_err > PENALTY_RATIO * MAX_ERROR:
+                this_err = (PENALTY_RATIO * MAX_ERROR +
+                            (this_err - PENALTY_RATIO * MAX_ERROR) * 10)
+            if this_err > MAX_ERROR:
+                errors += 1
+            result += this_err ** 2
+
+        for i in range(0, len(list(x))):
+            nexti = (i + 1) % len(list(x))
+            delta = abs(x[nexti] - x[i])
+            if delta > (PENALTY_RATIO * MAX_STEP_CHANGE):
+                result += (
+                    10 * (delta - PENALTY_RATIO * MAX_STEP_CHANGE) ** 2)
+        return result, errors
+
+    def metric(x, *args):
+        result = full_metric(x, *args)[0]
+        print(f"optimizing - metric={result:.5f}     ", end='\r', flush=True)
+        return result
+
+    starting_metric, starting_errors = full_metric(offset)
+
+    if (force_optimize or (
+            allow_optimize and
+            (starting_metric > 30 or
+             starting_errors > 0))):
+        print()
+        # Optimize these initial offsets.
+        optimres = scipy.optimize.minimize(metric, offset, tol=1e1)
+
+        if not optimres.success:
+            result.errors.append(
+                f"optimization failed {result.message}")
+
+        print()
+        offset = list(optimres.x)
+
+    result.fit_metric, _ = full_metric(offset)
+
+    # Now double check our results.
+    resampled_offset = _interpolate(xpos, offset_x + [65536],
+                                    offset + [offset[0]])
+    any_sample_error = False
+    sample_errors = []
+    for a, b in zip(err, resampled_offset):
+        sample_error = _wrap_neg_pi_to_pi(a - b)
+        sample_errors.append(sample_error)
+        if not any_sample_error and abs(sample_error) > MAX_ERROR:
+            result.errors.append(
+                f"excessive error in curve fit |{sample_error}| > {MAX_ERROR}")
+            any_sample_error = True
+
     result.offset = offset
 
     result.debug = {
@@ -340,6 +419,9 @@ def calibrate(parsed,
         'avg_interp' : avg_interp,
         'err' : err,
         'avg_err' : avg_err,
+        'offset_x' : offset_x,
+        'offset' : offset,
+        'sample_errors' : sample_errors,
     }
 
     return result

{moteus-0.3.71 → moteus-0.3.72}/moteus/moteus.py

@@ -530,7 +530,7 @@ class Result:
     id = None
     arbitration_id = None
     bus = None
-    values = []
+    values = {}
 
     def __repr__(self):
         value_str = ', '.join(['{}(0x{:03x}): {}'.format(Register(key).name, key, value)

{moteus-0.3.71 → moteus-0.3.72}/moteus/moteus_tool.py

@@ -44,6 +44,14 @@ MAX_FLASH_BLOCK_SIZE = 32
 FIND_TARGET_TIMEOUT = 0.01 if sys.platform != 'win32' else 0.05
 
 
+def _wrap_neg_pi_to_pi(value):
+    while value > math.pi:
+        value -= 2.0 * math.pi
+    while value < -math.pi:
+        value += 2.0 * math.pi
+    return value
+
+
 class FirmwareUpgrade:
     '''This encodes "magic" rules about upgrading firmware, largely about
     how to munge configuration options so as to not cause behavior
@@ -54,7 +62,7 @@ class FirmwareUpgrade:
         self.old = old
         self.new = new
 
-        SUPPORTED_ABI_VERSION = 0x0108
+        SUPPORTED_ABI_VERSION = 0x0109
 
         if new > SUPPORTED_ABI_VERSION:
             raise RuntimeError(f"\nmoteus_tool needs to be upgraded to support this firmware\n\n (likely 'python -m pip install --upgrade moteus')\n\nThe provided firmare is ABI version 0x{new:04x} but this moteus_tool only supports up to 0x{SUPPORTED_ABI_VERSION:04x}")
@@ -307,6 +315,35 @@ class FirmwareUpgrade:
                 print("Upgrading servo.bemf_feedforward to 0.0")
                 items[b'servo.bemf_feedforward'] = b'0.0'
 
+        if self.new >= 0x0109 and self.old <= 0x0108:
+            # Try to fix up the motor commutation offset tables.
+            old_offsets = []
+            i = 0
+            while True:
+                key = f'motor.offset.{i}'.encode('utf8')
+                if key not in items:
+                    break
+                old_offsets.append(float(items.get(key)))
+                i += 1
+
+            offsets = old_offsets[:]
+
+            # Unwrap this, then re-center the whole thing around 0.
+            for i in range(1, len(offsets)):
+                offsets[i] = (offsets[i - 1] +
+                              _wrap_neg_pi_to_pi(offsets[i] -
+                                                 offsets[i - 1]))
+            mean_offset = sum(offsets) / len(offsets)
+            delta = mean_offset - _wrap_neg_pi_to_pi(mean_offset)
+            offsets = [x - delta for x in offsets]
+
+            if any([abs(a - b) > 0.01
+                    for a, b in zip(offsets, old_offsets)]):
+                print("Re-wrapping motor commutation offsets")
+                for i in range(len(offsets)):
+                    key = f'motor.offset.{i}'.encode('utf8')
+                    items[key] = f'{offsets[i]}'.encode('utf8')
+
         lines = [key + b' ' + value for key, value in items.items()]
         return b'\n'.join(lines)
 
@@ -1002,6 +1039,10 @@ class Stream:
             raise RuntimeError(
                 'hall effect calibration requires specifying --cal-motor-poles')
 
+        if (self.args.cal_motor_poles % 2) == 1:
+            raise RuntimeError(
+                'only motors with even numbers of poles are supported')
+
         commutation_source = await self.read_config_int(
             "motor_position.commutation_source")
         aux_number = await self.read_config_int(
@@ -1062,21 +1103,26 @@ class Stream:
         await self.command("d stop")
 
     async def ensure_valid_theta(self, encoder_cal_voltage):
-        try:
-            # We might need to have some sense of pole count first.
-            if await self.read_config_double("motor.poles") == 0:
-                # Pick something arbitrary for now.
-                await self.command(f"conf set motor.poles 2")
+        # We might need to have some sense of pole count first.
+        if await self.read_config_double("motor.poles") == 0:
+            # Pick something arbitrary for now.
+            await self.command(f"conf set motor.poles 2")
 
+        try:
             motor_position = await self.read_data("motor_position")
-            if motor_position.homed == 0 and not motor_position.theta_valid:
-                # We need to find an index.
-                await self.find_index(encoder_cal_voltage)
         except RuntimeError:
             # Odds are we don't support motor_position, in which case
             # theta is always valid for the older versions that don't
             # support it.
-            pass
+            return
+
+        if motor_position.error != 0:
+            raise RuntimeError(
+                f"encoder error: {repr(motor_position.error)}")
+
+        if motor_position.homed == 0 and not motor_position.theta_valid:
+            # We need to find an index.
+            await self.find_index(encoder_cal_voltage)
 
     async def calibrate_encoder_mapping_absolute(self, encoder_cal_voltage):
         await self.ensure_valid_theta(encoder_cal_voltage)
@@ -1119,7 +1165,10 @@ class Stream:
             cal_file,
             desired_direction=1 if not self.args.cal_invert else -1,
             max_remainder_error=self.args.cal_max_remainder,
-            allow_phase_invert=allow_phase_invert)
+            allow_phase_invert=allow_phase_invert,
+            allow_optimize=not self.args.cal_disable_optimize,
+            force_optimize=self.args.cal_force_optimize,
+        )
 
         if cal_result.errors:
             raise RuntimeError(f"Error(s) calibrating: {cal_result.errors}")
@@ -1360,9 +1409,19 @@ class Stream:
             return 1 if x >= 0 else -1
 
         velocity_samples = []
+        power_samples = []
 
         while True:
             data = await self.read_servo_stats()
+
+            total_current_A = math.hypot(data.d_A, data.q_A)
+            total_power_W = voltage * total_current_A
+
+            power_samples.append(total_power_W)
+
+            if len(power_samples) > AVERAGE_COUNT:
+                del power_samples[0]
+
             velocity_samples.append(data.velocity)
 
             if len(velocity_samples) > (3 * AVERAGE_COUNT):
@@ -1374,6 +1433,24 @@ class Stream:
             if (time.time() - start_time) > 2.0:
                 return recent_average
 
+            if len(power_samples) >= AVERAGE_COUNT:
+                average_power_W = sum(power_samples) / len(power_samples)
+                max_power_W = (self.args.cal_max_kv_power_factor *
+                               self.args.cal_motor_power)
+
+                # This is a safety.  During speed measurement, current
+                # should always be near 0.  However, if the encoder
+                # commutation calibration failed, we can sometimes
+                # trigger large currents during the Kv detection phase
+                # while not actually moving.
+                if (abs(recent_average) < 0.2 and
+                    average_power_W > max_power_W):
+                    await self.command("d stop")
+
+                    raise RuntimeError(
+                        f"Motor failed to spin, {average_power_W} > " +
+                        f"{max_power_W}")
+
             if (len(velocity_samples) >= AVERAGE_COUNT and
                 abs(recent_average) < 0.2):
                 return recent_average
@@ -1765,11 +1842,17 @@ async def async_main():
     parser.add_argument('--cal-force-kv', metavar='Kv', type=float,
                         default=None,
                         help='do not calibrate Kv, but use the specified value')
+    parser.add_argument('--cal-force-optimize', action='store_true',
+                        help='require nonlinear commutation optimization')
+    parser.add_argument('--cal-disable-optimize', action='store_true',
+                        help='prevent nonlinear commutation optimization')
 
 
     parser.add_argument('--cal-max-remainder', metavar='F',
                         type=float, default=0.1,
                         help='maximum allowed error in calibration')
+    parser.add_argument('--cal-max-kv-power-factor', type=float,
+                        default=1.0)
     parser.add_argument('--cal-raw', metavar='FILE', type=str,
                         help='write raw calibration data')
 

{moteus-0.3.71 → moteus-0.3.72}/moteus/version.py

@@ -12,4 +12,4 @@
 # See the License for the specific language governing permissions and
 # limitations under the License.
 
-VERSION="0.3.71"
+VERSION="0.3.72"

{moteus-0.3.71 → moteus-0.3.72}/moteus.egg-info/PKG-INFO

@@ -1,6 +1,6 @@
 Metadata-Version: 2.1
 Name: moteus
-Version: 0.3.71
+Version: 0.3.72
 Summary: moteus brushless controller library and tools
 Home-page: https://github.com/mjbots/moteus
 Author: mjbots Robotic Systems
@@ -15,8 +15,10 @@ Description-Content-Type: text/markdown
 Requires-Dist: pyserial>=3.5
 Requires-Dist: python-can>=3.3
 Requires-Dist: pyelftools>=0.26
+Requires-Dist: scipy>=1.8.0
 Requires-Dist: importlib_metadata>=3.6
 Requires-Dist: pywin32; platform_system == "Windows"
+Requires-Dist: numpy<2
 
 # Python bindings for moteus brushless controller #
 

{moteus-0.3.71 → moteus-0.3.72}/moteus.egg-info/requires.txt

@@ -1,7 +1,9 @@
 pyserial>=3.5
 python-can>=3.3
 pyelftools>=0.26
+scipy>=1.8.0
 importlib_metadata>=3.6
+numpy<2
 
 [:platform_system == "Windows"]
 pywin32

{moteus-0.3.71 → moteus-0.3.72}/setup.py

@@ -25,7 +25,7 @@ long_description = (here / 'README.md').read_text(encoding='utf-8')
 
 setuptools.setup(
     name = 'moteus',
-    version = "0.3.71",
+    version = "0.3.72",
     description = 'moteus brushless controller library and tools',
     long_description = long_description,
     long_description_content_type = 'text/markdown',
@@ -50,7 +50,9 @@ setuptools.setup(
         'pyserial>=3.5',
         'python-can>=3.3',
         'pyelftools>=0.26',
+        'scipy>=1.8.0',
         'importlib_metadata>=3.6',
         'pywin32;platform_system=="Windows"',
+        'numpy<2',
     ],
 )