Added sample gamepad control

.flake8

@@ -0,0 +1,3 @@
+[flake8]
+max-line-length=140
+ignore=E261, E266, E302, E265

examples/gamepad.py

@@ -0,0 +1,46 @@
+try:
+    import inputs
+except:
+    print('This sample needs the inputs package (pip install inputs).')
+    exit()
+
+from roman import *
+import math
+
+def normalize_thumb_value(v):
+    # eliminate deadzone and normalize
+    return (v - 8000 * v/abs(v) if abs(v) > 8000 else 0) / (32768.0 - 8000)
+
+def get_gamepad_state():
+    return inputs.devices.gamepads[0]._GamePad__read_device().gamepad
+
+if __name__ == '__main__':
+    robot = connect(use_sim=True)
+    done = False
+    print("Use thumbsticks to move the arm.")
+    print("Use triggers to open/close gripper.")
+    print("Press the back button to exit.")
+
+    while not done:
+        gps = get_gamepad_state()
+        d0 = 0.1 * normalize_thumb_value(gps.r_thumb_x)
+        d1 = 0.1 * normalize_thumb_value(gps.r_thumb_y)
+        d2 = - (0.1 * normalize_thumb_value(gps.r_thumb_y) + 0.1 * normalize_thumb_value(gps.l_thumb_y))
+        d5 = 0.1 * normalize_thumb_value(gps.l_thumb_x)
+        pose = robot.arm.state.joint_positions()
+        target = pose + [d0, d1, d2, 0, 0, d5]
+        target[3] = -target[2] - target[1] - math.radians(90)
+        robot.arm.move(target, max_speed=3, max_acc=1, blocking=False)
+
+        if gps.left_trigger > robot.hand.state.position():
+            robot.hand.move(gps.left_trigger)
+        elif gps.right_trigger > 255 - robot.hand.state.position():
+            robot.hand.move(255 - gps.right_trigger)
+
+        if gps.buttons == 4096:
+            mode = hand.GraspMode.PINCH if robot.hand.state.mode() != hand.GraspMode.PINCH else hand.GraspMode.BASIC
+            robot.hand.set_mode(mode)
+
+        done = gps.buttons == 0x20 # back btn exits
+
+    robot.disconnect()

roman/robot.py

@@ -1,13 +1,12 @@
 import os
-import numpy as np 
+import numpy as np
 import threading
 import time
 from .rq import hand
 from . import rq
 from .ur import arm
 from . import ur
-from . import server 
-from .sim.ur_rq3 import SimEnv
+from . import server
 
 class Robot:
     '''
@@ -25,7 +24,7 @@ class Robot:
 
     def move_simple(self, dx, dy, dz, dyaw, gripper_state=hand.Position.OPENED, max_speed = 0.5):
         '''
-        Moves the arm relative to the current position in carthesian coordinates, 
+        Moves the arm relative to the current position in carthesian coordinates,
         assuming the gripper is vertical (aligned with the z-axis), pointing down.
         This supports the simplest Gym robotic manipulation environment.
         '''
@@ -37,7 +36,7 @@ class Robot:
 
     def step(self, dx, dy, dz, dyaw, gripper_state=hand.Position.OPENED, max_speed = 0.5, max_acc=0.5, dt = 0.2):
         '''
-        Moves the arm relative to the current position in carthesian coordinates, 
+        Moves the arm relative to the current position in carthesian coordinates,
         assuming the gripper is vertical (aligned with the z-axis), pointing down.
         This version returns after the amount of time specified by dt.
         This supports the simplest Gym robotic manipulation environment.
@@ -57,7 +56,7 @@ class Robot:
 
 def connect(use_sim = True, in_proc = False, sim_init = None):
     '''
-    Creates a robot instance with either sim or real (hardware) backing. 
+    Creates a robot instance with either sim or real (hardware) backing.
     By default, sim runs in-proc and real runs out-of-proc.
     Note that when running in-proc the async methods behave differently (since there's no server to execute them)
     and need to be called in a tight loop.

roman/server.py

@@ -15,7 +15,7 @@ def start(config):
 class InProcHost:
     def __init__(self, config):
         self.config = config
-        self.use_sim = config.get("use_sim", True) 
+        self.use_sim = config.get("use_sim", True)
 
     def start(self):
         if self.use_sim:
@@ -26,18 +26,18 @@ class InProcHost:
             configurator = self.config.get("sim.init", None)
             if configurator is not None:
                 configurator(self.env)
-            
+
         else:
-            self._arm_con = ur.Connection() 
+            self._arm_con = ur.Connection()
             self._hand_con = rq.Connection()
 
         self._arm_con.connect()
-        activate_hand = self.config.get("hand.activate", True) 
+        activate_hand = self.config.get("hand.activate", True)
         self._hand_con.connect(activate_hand)
         self.arm = ur.ArmController(self._arm_con)
         self.hand = rq.HandController(self._hand_con)
         return (self.arm, self.hand)
-    
+
     def stop(self):
         self._arm_con.disconnect()
         self._hand_con.disconnect()
@@ -61,14 +61,14 @@ class RemoteHostProxy:
         self.__shutdown_event = Event()
         self.__process = Process(target=server_loop, args=(arm_client, hand_client, self.__shutdown_event, self.config))
         self.__process.start()
-        self.arm =  RemoteHostProxy.PipeConnection(arm_server)
+        self.arm = RemoteHostProxy.PipeConnection(arm_server)
         self.hand = RemoteHostProxy.PipeConnection(hand_server)
         return (self.arm, self.hand)
 
     def stop(self):
         self.__shutdown_event.set()
         self.__process.join()
-     
+
 
 #************************************************************************************************
 # Server loop
@@ -94,11 +94,11 @@ def server_loop(arm_client, hand_client, shutdown_event, config={}, log_file=Non
         arm_cmd_is_new = arm_client.poll()
         if arm_cmd_is_new:
             arm_client.recv_bytes_into(arm_cmd.array) # blocking
-        
+
         hand_cmd_is_new = hand_client.poll()
         if hand_cmd_is_new:
             hand_client.recv_bytes_into(hand_cmd.array) # blocking
-        
+
         hand_ctrl.execute(hand_cmd, hand_state)
         arm_ctrl.execute(arm_cmd, arm_state)
 

roman/sim/simenv.py

@@ -22,6 +22,7 @@ class SimEnv:
         if self._useGUI:
             pb.connect(pb.GUI)
             pb.resetDebugVisualizerCamera(1.5, -30, -15, cameraTargetPosition=[-0.4, 0, 0.3])
+            pb.configureDebugVisualizer(pb.COV_ENABLE_GUI, self._useGUI)
         else:
             pb.connect(pb.DIRECT)
         self.reset()
@@ -33,10 +34,10 @@ class SimEnv:
     def update(self):
         pb.stepSimulation()
         self.__time += SimEnv.TIME_STEP
-        
+
     def time(self):
         return self.__time
-   
+
     def disconnect(self):
         pb.disconnect()
 
@@ -53,4 +54,3 @@ class SimEnv:
             pb.changeVisualShape(id, -1, rgbaColor=color)
         #pb.changeDynamics(id, -1, restitution = 0.5)
         return id
- 

roman/ur/controllers.py

@@ -33,8 +33,8 @@ class EMAForceCalibrator:
     '''
     Keeps an exponentially weighted moving average of the force reported by the FT sensor.
     Adds the average to the expected force of each move command.
-    Substracts the average from the reported sensor force of each response.  
-    avg(t) = alpha*sample(t) + (1-alpha)*avg(t-1) 
+    Substracts the average from the reported sensor force of each response.
+    avg(t) = alpha*sample(t) + (1-alpha)*avg(t-1)
     '''
     def __init__(self, next, alpha = 0.01):
         self.next = next
@@ -48,8 +48,8 @@ class EMAForceCalibrator:
         if cmd.is_move_command():
             np.add(self.force_average, cmd.force_low_bound(), self.cmd.force_low_bound().array)
             np.add(self.force_average, cmd.force_high_bound(), self.cmd.force_high_bound().array)
-        
-        self.next.execute(self.cmd, state)   
+
+        self.next.execute(self.cmd, state)
         if not state.is_contact():
             self.sample[:] = state.sensor_force()
             np.multiply(self.force_average, 1-self.alpha, self.force_average.array)
@@ -74,9 +74,9 @@ class TouchController:
         self.force_sum = np.zeros(6)
 
     def execute(self, cmd, state):
-        
+
         self.next.execute(cmd, state)
-        
+
         if state.is_goal_reached():
             # stopped because the arm reached the goal but didn't detect contact, so this is a failure
             state._set_state_flag(State._STATUS_FLAG_GOAL_REACHED, 0)
@@ -88,7 +88,7 @@ class TouchController:
             self.count = self.validation_count = cmd.contact_handling()
             self.force_sum[:] = 0
             return state
-        
+
         if state.is_moving() and not state.is_contact():
             return state
 
@@ -113,8 +113,8 @@ class TouchController:
 
 class ArmController:
     '''
-    This is the highest-level controller. 
-    It's job is to determine which controller hierarchy to set up for a given command.
+    This is the highest-level controller.
+    Its job is to determine which controller hierarchy to set up for a given command.
     '''
     def __init__(self, connection):
         basic = BasicController(connection)
@@ -129,4 +129,3 @@ class ArmController:
             self.cmd[:] = cmd
         controller = self.controllers[int(self.cmd.controller_flags())]
         return controller.execute(self.cmd, state)
- 

roman/ur/sim_connection.py

@@ -20,5 +20,4 @@ class SimConnection:
         """Sends the command to control layer and reads back the state, emulating the wire protocol used with the real robot."""
         state[:] = interface.execute_arm_command(cmd, 0)
         self.env.update() # note that the sim update is called twice, once here and once by the hand's sim_connection
-        
- 
+

setup.py

@@ -9,7 +9,7 @@ import setuptools
 setuptools.setup(
     name='roman',
     version='1.0',
-    url = "https://github.com/microsoft/roman",
+    url="https://github.com/microsoft/roman",
     install_requires=[
         "numpy", "scipy", "pybullet"
     ],