diff --git a/.flake8 b/.flake8 new file mode 100644 index 0000000..6349521 --- /dev/null +++ b/.flake8 @@ -0,0 +1,3 @@ +[flake8] +max-line-length=140 +ignore=E261, E266, E302, E265 diff --git a/examples/gamepad.py b/examples/gamepad.py new file mode 100644 index 0000000..d3387c3 --- /dev/null +++ b/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() diff --git a/roman/robot.py b/roman/robot.py index 2a072f3..6929815 100644 --- a/roman/robot.py +++ b/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. diff --git a/roman/server.py b/roman/server.py index 6e53ff7..fe56eea 100644 --- a/roman/server.py +++ b/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) diff --git a/roman/sim/simenv.py b/roman/sim/simenv.py index e69294c..924378e 100644 --- a/roman/sim/simenv.py +++ b/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 - \ No newline at end of file diff --git a/roman/ur/controllers.py b/roman/ur/controllers.py index 30bc819..12db737 100644 --- a/roman/ur/controllers.py +++ b/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) - \ No newline at end of file diff --git a/roman/ur/sim_connection.py b/roman/ur/sim_connection.py index f398530..82cd1ae 100644 --- a/roman/ur/sim_connection.py +++ b/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 - - \ No newline at end of file + diff --git a/setup.py b/setup.py index 7d01f43..393327e 100644 --- a/setup.py +++ b/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" ],