Develop (#16)

* R links234/update joystick interface (#15)

* Refactor `util.input.joystick` module to use proper SDL2 interface (also allows proper mapping of joysticks other than PS3 and PS4 controllers)
* Add Mobile IO controller support (`util.input.module_controller`), which can act as a drop in replacement for game controller
* Add Mobile IO support to Igor demo
* Igor demo now follows MATLAB demo _much_ more closely
* Fix various bugs in Igor demo
* Add Igor demo documentation and readme

* Make Mobile IO the default for Igor II demo

* Add initial install files for starting demo up on boot (GNU/Linux only)

kits/igor2/README.md

@@ -0,0 +1,38 @@
+# Igor Balancing Robot Matlab Demo
+
+## Requirements
+
+### Controller
+
+The demo can be run using either a gamecontroller or Mobile IO device acessible from the computer running the demo.
+
+* USB controller (Must be compatibile with SDL2 - check [here](https://github.com/gabomdq/SDL_GameControllerDB/blob/master/gamecontrollerdb.txt) for your controller)
+* Mobile IO (Requires HEBI's Mobile IO app on an Android or iOS device)
+
+### Software Requirements 
+* [HEBI Python API](https://pypi.org/project/hebi-py/)
+* [PySDL2](https://pypi.org/project/PySDL2/)
+  * **Linux Users:** You should check if your distro has a package for this already. For Ubuntu, install the [pysdl2 package](https://launchpad.net/ubuntu/+source/pysdl2)
+  * If not installing through a package manager, make sure you have the SDL2 library installed!
+
+## Running
+
+Simply run the `igor2_demo.py` file located at `kits/igor2`. Both Python 2 and 3 are supported, but note that Python 2 upstream will become [unsupported 1 Jan 2020.](https://pythonclock.org/)
+
+Note: By default, the demo will look for a Mobile IO device with family `HEBI` and name `Mobile IO`. You must provide `--joystick` if you want to control Igor using a USB gamecontroller.
+
+## Controls
+
+The demo provides default mappings for both supported controllers. You can modify them, if needed, by editing the `components/configuration.py` file directly.
+
+### Game Controller
+
+The default gamecontroller mappings are as followed. Note that the image is of a generic Sony Playstation 4 controller, but the concept applies to all other SDL2 compatible controllers.
+![ps4 igor](resources/ps4_igor.png)
+
+*Note: To exit idle mode, press L3 (press in left axis stick)*
+
+### Mobile IO
+
+The default Mobile IO mappings are as followed. Note that the layout of the application may appear different on your device than what is shown, but the buttons and axes are guaranteed across any device.
+![mobile io igor](resources/mobile_io_igor.png)

kits/igor2/components/arm.py

@@ -252,3 +252,18 @@ def set_wrist_velocity(self, value):
     self.acquire_value_lock()
     self._user_commanded_wrist_velocity = value
     self.release_value_lock()
+
+  def reset_state(self):
+    super(Arm, self).reset_state()
+
+    self._user_commanded_grip_velocity.fill(0)
+    self._current_xyz.fill(0)
+    np.copyto(self._grip_pos, self._home_ef[0:3, 3])
+    np.copyto(self._grip_pos, self._new_grip_pos)
+    np.copyto(self._joint_angles, self.home_angles.reshape((4, 1)))
+    self._joint_velocities.fill(0)
+    self._joint_efforts.fill(0)
+    self._user_commanded_wrist_velocity = 0.0
+    self._grav_comp_torque.fill(0)
+    self._current_det_actual = 0.0
+    self._current_det_expected = 0.0

kits/igor2/components/body.py

@@ -272,3 +272,32 @@ def update_position(self):
     masses = self._masses
     np.sum(np.multiply(self._current_xyz, matlib.repmat(masses.T, 3, 1)), axis=1, out=self._com)
     self._com *= 1.0/self.mass
+
+  def reset_state(self):
+    """
+    Used when transitioning Igor back into idle mode.
+    """
+    self._fbk_position.fill(0)
+    self._fbk_position_cmd.fill(0)
+    self._fbk_velocity.fill(0)
+    self._fbk_velocity_err.fill(0)
+    self._xyz_error.fill(0)
+    self._pos_error.fill(0)
+    self._vel_error.fill(0)
+    self._impedance_err.fill(0)
+    self._impedance_torque.fill(0)
+    self._current_tip_fk.fill(0)
+    self._current_j_actual.fill(0)
+    self._current_j_actual_f.fill(0)
+    self._current_j_expected.fill(0)
+    self._current_j_expected_f.fill(0)
+
+    if self._current_xyz is not None:
+      self._current_xyz.fill(0)
+
+    # TODO: is this necessary?
+    self._home_angles__flat = self._home_angles.A1
+    self._fbk_position__flat = self._fbk_position.A1
+    self._fbk_position_cmd__flat = self._fbk_position_cmd.A1
+    self._fbk_velocity__flat = self._fbk_velocity.A1
+    self._fbk_velocity_err__flat = self._fbk_velocity_err.A1

kits/igor2/components/chassis.py

@@ -23,7 +23,7 @@ def _create_trajectory(self):
                                              self._jerks)
 
   def __init__(self, val_lock):
-    super(Chassis, self).__init__(val_lock, mass=6.0, com=[0.0, 0.0, 0.10+0.3])
+    super(Chassis, self).__init__(val_lock, mass=6.0, com=[-0.05, 0.0, 0.10+0.3])
     self._user_commanded_directional_velocity = 0.0
     self._user_commanded_yaw_velocity = 0.0
     self._min_ramp_time = 0.5
@@ -79,6 +79,11 @@ def update_trajectory(self, user_commanded_knee_velocity, user_commanded_grip_ve
     t = time_now - self._time_s
     self._time_s = time_now
 
+    if t > self._trajectory.end_time:
+      # Clamp to end time to prevent undesired trajectory calculations.
+      # This can lead to issues when re-entering running mode after entering idle mode from running mode
+      t = self._trajectory.end_time
+
     # ---------------------------------------------
     # Smooth the trajectories for various commands.
     # This will be the starting waypoint for the new trajectory.
@@ -294,3 +299,16 @@ def set_yaw_velocity(self, velocity):
     self._user_commanded_yaw_velocity = velocity
     self.release_value_lock()
 
+  def reset_state(self):
+    self._user_commanded_directional_velocity = 0.0
+    self._user_commanded_yaw_velocity = 0.0
+    self._hip_pitch = 0.0
+    self._hip_pitch_velocity = 0.0
+    self._velocity_feedforward = 0.0
+    self._velocity_error = 0.0
+    self._velocity_error_cumulative = 0.0
+    self._lean_angle_error = 0.0
+    self._lean_angle_error_cumulative = 0.0
+    self._cmd_chassis_vel_last = 0.0
+    self._fbk_chassis_vel_last = 0.0
+    self._calculated_lean_angle = 0.0

kits/igor2/components/configuration.py

@@ -1,9 +1,10 @@
 import os
 
 from util.input.joystick import Joystick
+from util.input.module_controller import HebiModuleController
 
 # ------------------------------------------------------------------------------
-# Joystick selectors
+# Controller selectors
 # ------------------------------------------------------------------------------
 
 
@@ -35,7 +36,21 @@ def _joystick_by_name_selector(name):
   return None
 
 
-def _create_joystick_selector(strategy, arg=None):
+def _controller_by_mobile_io_selector(family, name, feedback_frequency):
+  import hebi
+  from time import sleep
+  lookup = hebi.Lookup()
+  sleep(2)
+  group = lookup.get_group_from_names([family], [name])
+  if group is None:
+    msg = 'Could not find Mobile IO on network with\nfamily: {0}\nname: {1}'.format(family, name)
+    print(msg)
+    raise RuntimeError(msg)
+  group.feedback_frequency = feedback_frequency
+  return HebiModuleController(group)
+
+
+def _create_controller_selector(strategy, arg=None):
   if strategy == 'first':
     return _joystick_first_available_selector
   elif strategy == 'index':
@@ -44,14 +59,127 @@ def _create_joystick_selector(strategy, arg=None):
   elif strategy == 'name':
     assert arg is not None
     return lambda: _joystick_by_name_selector(arg)
+  elif strategy == 'mobile_io':
+    assert arg is not None
+    return lambda: _controller_by_mobile_io_selector(arg[0], arg[1], arg[2])
   else:
-    raise RuntimeError('Invalid strategy {0}'.format(strategy))
+    raise RuntimeError('Invalid controller selector strategy {0}'.format(strategy))
+
+
+# ------------------------------------------------------------------------------
+# Controller Mappings
+# ------------------------------------------------------------------------------
+
+
+class IgorControllerMapping(object):
+  __slots__ = ('_arm_vel_x', '_arm_vel_y', '_stance_height', '_wrist_vel',
+    '_chassis_yaw', '_chassis_vel', '_exit_idle_modle', '_quit', '_balance_controller_toggle',
+    '_soft_shutdown', '_lower_arm', '_raise_arm', '_stance_height_control_strategy', 
+    '_wrist_velocity_control_strategy')
+
+  def __init__(self, arm_vel_x, arm_vel_y, stance_height, wrist_vel, chassis_yaw,
+    chassis_vel, exit_idle_modle, quit, balance_controller_toggle, soft_shutdown,
+    lower_arm, raise_arm, stance_height_control_strategy, wrist_velocity_control_strategy):
+
+    self._arm_vel_x = arm_vel_x
+    self._arm_vel_y = arm_vel_y
+    self._stance_height = stance_height
+    self._wrist_vel = wrist_vel
+    self._chassis_yaw = chassis_yaw
+    self._chassis_vel = chassis_vel
+    self._exit_idle_modle = exit_idle_modle
+    self._quit = quit
+    self._balance_controller_toggle = balance_controller_toggle
+    self._soft_shutdown = soft_shutdown
+    self._lower_arm = lower_arm
+    self._raise_arm = raise_arm
+    if stance_height_control_strategy == 'SLIDER':
+      if type(stance_height) is not str:
+        raise TypeError('stance_height must be a string for `SLIDER` stance height control strategy')
+    elif stance_height_control_strategy == 'TRIGGERS':
+      if type(stance_height) is not tuple:
+        raise TypeError('stance_height must be a tuple for `TRIGGERS` stance height control strategy')
+    else:
+      raise ValueError('Invalid stance height control strategy {0}'.format(stance_height_control_strategy))
+
+    if wrist_velocity_control_strategy == 'SLIDER':
+      if type(wrist_vel) is not str:
+        raise TypeError('wrist_vel must be a string for `SLIDER` wrist velocity control strategy')
+    elif wrist_velocity_control_strategy == 'BUTTONS':
+      if type(wrist_vel) is not tuple:
+        raise TypeError('wrist_vel must be a tuple for `BUTTONS` wrist velocity control strategy')
+    else:
+      raise ValueError('Invalid wrist velocity control strategy {0}'.format(wrist_velocity_control_strategy))
+
+    self._wrist_velocity_control_strategy = wrist_velocity_control_strategy
+    self._stance_height_control_strategy = stance_height_control_strategy
+
+  @property
+  def arm_vel_x(self):
+    return self._arm_vel_x
+
+  @property
+  def arm_vel_y(self):
+    return self._arm_vel_y
+
+  @property
+  def stance_height(self):
+    return self._stance_height
+
+  @property
+  def wrist_vel(self):
+    return self._wrist_vel
+
+  @property
+  def chassis_yaw(self):
+    return self._chassis_yaw
+
+  @property
+  def chassis_vel(self):
+    return self._chassis_vel
+
+  @property
+  def exit_idle_modle(self):
+    return self._exit_idle_modle
+
+  @property
+  def quit(self):
+    return self._quit
+
+  @property
+  def balance_controller_toggle(self):
+    return self._balance_controller_toggle
+
+  @property
+  def soft_shutdown(self):
+    return self._soft_shutdown
+
+  @property
+  def lower_arm(self):
+    return self._lower_arm
+
+  @property
+  def raise_arm(self):
+    return self._raise_arm
+
+  @property
+  def stance_height_control_strategy(self):
+    return self._stance_height_control_strategy
+
+  @property
+  def wrist_velocity_control_strategy(self):
+    return self._wrist_velocity_control_strategy
+
+
+_default_joystick_mapping = IgorControllerMapping('LEFT_STICK_Y', 'LEFT_STICK_X', ('L2', 'R2'), ('DPAD_DOWN', 'DPAD_UP'), 'RIGHT_STICK_X', 'RIGHT_STICK_Y', 'L3', 'SHARE', 'TOUCHPAD', 'OPTIONS', 'L1', 'R1', 'TRIGGERS', 'BUTTONS')
+_default_mobile_io_mapping = IgorControllerMapping('a2', 'a1', 'a3', 'a6', 'a7', 'a8', 'b3', 'b1', 'b2', 'b4', 'b8', 'b6', 'SLIDER', 'SLIDER')
 
 
 # ------------------------------------------------------------------------------
 # Configuration
 # ------------------------------------------------------------------------------
 
+
 class Igor2Config(object):
   """
   Used when starting up Igor II.
@@ -74,74 +202,66 @@ def __init__(self, imitation=False):
     self.__default_gains_no_cam = os.path.join(resource_path, 'igorGains_noCamera.xml')
     self.__imitation = imitation
     self.__find_joystick_strategy = None
+    self.__controller_mapping = None
     self.select_first_available_joystick()
 
   def select_joystick_by_name(self, name):
     """
     Set the joystick selection strategy to select a joystick by name
     """
-    self.__find_joystick_strategy = _create_joystick_selector('name', name)
+    self.__find_joystick_strategy = _create_controller_selector('name', name)
+    self.__controller_mapping = _default_joystick_mapping
 
   def select_joystick_by_index(self, index):
     """
     Set the joystick selection strategy to select a joystick by index
     """
-    self.__find_joystick_strategy = _create_joystick_selector('index', index)
+    self.__find_joystick_strategy = _create_controller_selector('index', index)
+    self.__controller_mapping = _default_joystick_mapping
 
   def select_first_available_joystick(self):
     """
     Set the joystick selection strategy to select the first available joystick
     """
-    self.__find_joystick_strategy = _create_joystick_selector('first')
+    self.__find_joystick_strategy = _create_controller_selector('first')
+    self.__controller_mapping = _default_joystick_mapping
 
-  @property
-  def module_names(self):
+  def select_controller_by_mobile_io(self, family, name, feedback_frequency=200):
     """
-    :return:
-    :rtype:  list
+    Set the joystick selection strategy to select a mobile IO module with the provided family and name
     """
+    self.__find_joystick_strategy = _create_controller_selector('mobile_io', (family, name, feedback_frequency))
+    self.__controller_mapping = _default_mobile_io_mapping
+
+  @property
+  def module_names(self):
     return self.__module_names
 
   @property
   def module_names_no_cam(self):
-    """
-    :return:
-    :rtype:  list
-    """
     return self.__module_names[0:-1]
 
   @property
   def family(self):
-    """
-    :return:
-    :rtype:  str
-    """
     return self.__family
 
   @property
   def gains_xml(self):
-    """
-    :return:
-    :rtype:  str
-    """
     return self.__default_gains
 
   @property
   def gains_no_camera_xml(self):
-    """
-    :return:
-    :rtype:  str
-    """
     return self.__default_gains_no_cam
 
   @property
   def is_imitation(self):
-    """
-    :return:
-    :rtype:  bool
-    """
     return self.__imitation
 
   @property
   def joystick_selector(self):
     return self.__find_joystick_strategy
+
+  @property
+  def controller_mapping(self):
+    return self.__controller_mapping
+