Source code for pypot.dynamixel.controller

import time
import logging

from .io.abstract_io import DxlError
from ..robot.controller import MotorsController

logger = logging.getLogger(__name__)

[docs]class DxlController(MotorsController): def __init__(self, io, motors, sync_freq, synchronous, mode, regname, varname=None): MotorsController.__init__(self, io, motors, sync_freq) self.ids = [ for m in self.working_motors] self.synchronous = synchronous self.mode = mode self.regname = regname self.varname = regname if varname is None else varname for m in motors: if mode == 'get': m._read_synchronous[self.varname] = self.synchronous else: m._write_synchronous[self.varname] = self.synchronous @property def working_motors(self): return [m for m in self.motors if not m._broken] @property def synced_motors(self): motors = [m for m in self.working_motors if self.varname in m.registers] if self.synchronous: motors = ([m for m in motors if m._read_synced[self.varname].needed] if self.mode == 'get' else [m for m in motors if m._write_synced[self.varname].needed]) return motors
[docs] def setup(self): if self.mode == 'set': MAX_TRIALS = 25 for _ in range(MAX_TRIALS): if self.get_register(self.working_motors, disable_sync_read=True): break time.sleep(0.1) else: raise IOError('Cannot initialize syncloop for "{}". You need to desactivate sync_read if you use a usb2dynamixel device. '.format( self.regname))
[docs] def update(self): if not self.synced_motors: return return (self.get_register(self.synced_motors) if self.mode == 'get' else self.set_register(self.synced_motors))
[docs] def get_register(self, motors, disable_sync_read=False): """ Gets the value from the specified register and sets it to the :class:`~pypot.dynamixel.motor.DxlMotor`. """ if not motors: return False ids = [ for m in motors] getter = getattr(, 'get_{}'.format(self.regname)) values = (sum([list(getter([id])) for id in ids], []) if disable_sync_read else getter(ids)) if not values: return False for m, val in zip(motors, values): m.__dict__[self.varname] = val for m in motors: m._read_synced[self.varname].done() return True
[docs] def set_register(self, motors): """ Gets the value from :class:`~pypot.dynamixel.motor.DxlMotor` and sets it to the specified register. """ if not motors: return ids = [ for m in motors] values = (m.__dict__[self.varname] for m in motors) getattr(, 'set_{}'.format(self.regname))(dict(zip(ids, values))) for m in motors: m._write_synced[self.varname].done()
[docs]class AngleLimitRegisterController(DxlController): def __init__(self, io, motors, sync_freq, synchronous): DxlController.__init__(self, io, motors, sync_freq, synchronous, 'get', 'angle_limit') self.varnames = ['lower_limit', 'upper_limit'] for m in motors: for var in self.varnames: m._read_synchronous[var] = self.synchronous @property def synced_motors(self): motors = self.working_motors if self.synchronous: sync_motors = [] for m in motors: for var in self.varnames: if m._read_synced[var].needed: sync_motors.append(m) motors = sync_motors return motors
[docs] def get_register(self, motors): if not motors: return ids = [ for m in motors] values = for m, val in zip(motors, values): m.__dict__['lower_limit'], m.__dict__['upper_limit'] = val for m in motors: for var in ['lower_limit', 'upper_limit']: m._read_synced[var].done()
[docs]class PosSpeedLoadDxlController(DxlController): def __init__(self, io, motors, sync_freq): DxlController.__init__(self, io, motors, sync_freq, False, 'get', 'present_position')
[docs] def setup(self): torques = for m, c in zip(self.working_motors, torques): m.compliant = not c self._old_torques = torques self._old_goals = { 0.0 for m in self.motors} try: values = positions, speeds, loads = zip(*values) except ValueError: raise DxlError("Couldn't initialize pos/speed/load sync loop!") for m, p, s, l in zip(self.working_motors, positions, speeds, loads): m.__dict__['goal_position'] = p m.__dict__['moving_speed'] = s m.__dict__['torque_limit'] = l
[docs] def update(self): self.get_present_position_speed_load(self.working_motors) self.set_goal_position_speed_load(self.working_motors)
[docs] def get_present_position_speed_load(self, motors): ids = [ for m in motors] values = if not values: logger.warning('Timeout when getting pos/speed/load from %s', ids) return positions, speeds, loads = zip(*values) for m, p, s, l in zip(motors, positions, speeds, loads): m.__dict__['present_position'] = p m.__dict__['present_speed'] = s m.__dict__['present_load'] = l
[docs] def set_goal_position_speed_load(self, motors): change_torque = {} torques = [not m.compliant for m in motors] for m, t, old_t in zip(motors, torques, self._old_torques): if t != old_t: change_torque[] = t self._old_torques = torques if change_torque: rigid_motors = [] for m in motors: # Filter force control motors - only update values if goal_position has changed if getattr(m, "force_control_enable", False) and not m.compliant and self._old_goals[] != m.__dict__['goal_position']: rigid_motors += [m] self._old_goals[] = m.__dict__['goal_position'] # Do not filter motors without force control elif not m.compliant: rigid_motors += [m] ids = tuple( for m in rigid_motors) if not ids: return values = ((m.__dict__['goal_position'], m.__dict__['moving_speed'], m.__dict__['torque_limit']) for m in rigid_motors), values)))