moteus 0.3.70__py3-none-any.whl → 0.3.72__py3-none-any.whl

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/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/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,15 +62,23 @@ class FirmwareUpgrade:
         self.old = old
         self.new = new
 
-        SUPPORTED_ABI_VERSION = 0x0107
+        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}")
 
+        if old > SUPPORTED_ABI_VERSION:
+            raise RuntimeError(f"\nmoteus_tool needs to be upgraded to support this board\n\n (likely 'python -m pip install --upgrade moteus')\n\nThe board firmware is ABI version 0x{old:04x} but this moteus_tool only supports up to 0x{SUPPORTED_ABI_VERSION:04x}")
+
     def fix_config(self, old_config):
         lines = old_config.split(b'\n')
         items = dict([line.split(b' ') for line in lines if b' ' in line])
 
+        if self.new <= 0x0107 and self.old >= 0x0108:
+            if float(items.get(b'servo.bemf_feedforward', '0')) == 0.0:
+                print("Reverting servo.bemf_feedforward to 1.0")
+                items[b'servo.bemf_feedforward'] = b'1.0'
+
         if self.new <= 0x0106 and self.old >= 0x0107:
             # motor_position.output.sign was broken in older versions.
             if int(items[b'motor_position.output.sign']) != 1:
@@ -294,6 +310,40 @@ class FirmwareUpgrade:
             # No actual configuration updating is required here.
             pass
 
+        if self.new >= 0x0108 and self.old <= 0x0107:
+            if float(items.get(b'servo.bemf_feedforward', 1.0)) == 1.0:
+                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)
 
@@ -989,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(
@@ -1049,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)
@@ -1106,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}")
@@ -1347,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):
@@ -1361,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
@@ -1752,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/version.py

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

{moteus-0.3.70.dist-info → moteus-0.3.72.dist-info}/METADATA

@@ -1,6 +1,6 @@
 Metadata-Version: 2.1
 Name: moteus
-Version: 0.3.70
+Version: 0.3.72
 Summary: moteus brushless controller library and tools
 Home-page: https://github.com/mjbots/moteus
 Author: mjbots Robotic Systems
@@ -15,7 +15,9 @@ 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: numpy <2
 Requires-Dist: pywin32 ; platform_system == "Windows"
 
 # Python bindings for moteus brushless controller #

{moteus-0.3.70.dist-info → moteus-0.3.72.dist-info}/RECORD

@@ -1,12 +1,12 @@
 moteus/__init__.py,sha256=DxasQre5HmK3BS3I35K8GEUvFZF3_OE_hjVzQy-seIE,739
 moteus/aioserial.py,sha256=GeWuvsZKCRrfBN33JZFjtBXPr-0sKpQv9shRn2ulcDA,1079
 moteus/aiostream.py,sha256=YAkVF6QWsA49vqO-GgXEohDghqm_-nnajJzhO_Q9qNQ,3696
-moteus/calibrate_encoder.py,sha256=R3pRgGqrDCcmKQqFE7Fr4p8TSie179iqNfBwEJS3pL0,12416
+moteus/calibrate_encoder.py,sha256=6ecNPY1CnCgoa7z9YmeIK471-TQLirNbz_B5erBGarU,15115
 moteus/command.py,sha256=UkOsbtkso6Oyex8CfbpAKpBNriik519ymxL86EZGkRs,1169
 moteus/export.py,sha256=vRIfldaqz1eHtWo3993SvatATtu73TZbejL0hzQe8YE,1646
 moteus/fdcanusb.py,sha256=7PrQiCTROY96gdT2zSZYU1bOCriw-I7H6NspaZpiEx4,7431
-moteus/moteus.py,sha256=gARNQ2_irh_1zp5YBTXmZK5U7SziBna8R140e_Wyfxk,48998
-moteus/moteus_tool.py,sha256=dawmuZ9SnmZ9rDpKMnnGxeZkadrTaMGPEfs13X7nLP0,70999
+moteus/moteus.py,sha256=GFNGB4XedCOLJSr_ukhvIsGfY1y5p1y-7nUKyFfuOl8,48998
+moteus/moteus_tool.py,sha256=ZdUisFMTy_d5OYqTltSRlung6Zf-5ezQjylgaGSYnjw,75146
 moteus/multiplex.py,sha256=LF6MuelzYHqqsCJuCB9YeEyUA03eBaTYRwAVotX3qm8,10120
 moteus/posix_aioserial.py,sha256=2oDrw8TBEwuEQjY41g9rHeuFeffcPHqMwNS3nf5NVq8,3137
 moteus/pythoncan.py,sha256=ofotOrDuaFhTLvaokaO3EJK6quVc75Bq-ue70lDMtXI,4071
@@ -14,10 +14,10 @@ moteus/reader.py,sha256=9i1-h4aGd4syfqtWJcpg70Bl-bmunkGU4FmXmOLyRt8,12121
 moteus/regression.py,sha256=M5gjDBYJQ64iBXIrvBhMkD8TYhtlnQ85x8U4py0niGA,1196
 moteus/router.py,sha256=501W5GZ12rFoc1lmcH3S7IYsoc-Q_-FJ4B3i37RzE3Q,2061
 moteus/transport.py,sha256=WhkW2G9i25lkOlO55eI5_oXmU0PhDmxTeJ75Sg_7nTI,1021
-moteus/version.py,sha256=lm6BoZJ_qZ6gZJ22LrCQaLKZWXRuR2fV_to4JsV9Uvs,627
+moteus/version.py,sha256=QAXlfPdKUv7iVUDMlfA0IophEOfw4S5Pf7yO4Kiq5k4,627
 moteus/win32_aioserial.py,sha256=culdl-vYxBKD5n2s5LkIMGyUaHyCcEc8BL5-DWEaxX8,2025
-moteus-0.3.70.dist-info/METADATA,sha256=GgWzsYPbYst5VXP3tSE15HJuvpZgVAIAdIBpOySqHAc,3388
-moteus-0.3.70.dist-info/WHEEL,sha256=yQN5g4mg4AybRjkgi-9yy4iQEFibGQmlz78Pik5Or-A,92
-moteus-0.3.70.dist-info/entry_points.txt,sha256=accRcwir_K8wCf7i3qHb5R6CPh5SiSgd5a1A92ibb9E,56
-moteus-0.3.70.dist-info/top_level.txt,sha256=aZzmI_yecTaDrdSp29pTJuowaSQ9dlIZheQpshGg4YQ,7
-moteus-0.3.70.dist-info/RECORD,,
+moteus-0.3.72.dist-info/METADATA,sha256=ulIsuC8AzRG9A9h-prW5cKfBLihFDiR-K9xxBGGvHuw,3441
+moteus-0.3.72.dist-info/WHEEL,sha256=yQN5g4mg4AybRjkgi-9yy4iQEFibGQmlz78Pik5Or-A,92
+moteus-0.3.72.dist-info/entry_points.txt,sha256=accRcwir_K8wCf7i3qHb5R6CPh5SiSgd5a1A92ibb9E,56
+moteus-0.3.72.dist-info/top_level.txt,sha256=aZzmI_yecTaDrdSp29pTJuowaSQ9dlIZheQpshGg4YQ,7
+moteus-0.3.72.dist-info/RECORD,,