├── .github └── workflows │ ├── build.yml │ ├── lint.yml │ ├── mypy.yml │ └── publish.yml ├── .gitignore ├── .readthedocs.yml ├── LICENSE ├── README.md ├── blue_interface ├── __init__.py ├── blue_interface.py └── rosbridge_client.py ├── docs ├── .gitignore ├── Makefile ├── _config.yml └── source │ ├── conf.py │ └── index.rst ├── examples ├── gripper_controller.py ├── inverse_kinematics.py ├── print_status.py └── zero_position.py ├── mypy.ini ├── setup.py └── tests └── test_basic.py /.github/workflows/build.yml: -------------------------------------------------------------------------------- 1 | name: build 2 | 3 | on: 4 | push: 5 | branches: [master] 6 | pull_request: 7 | branches: [master] 8 | 9 | jobs: 10 | build: 11 | runs-on: ubuntu-latest 12 | strategy: 13 | matrix: 14 | python-version: ["2.7", "3.5", "3.6", "3.7", "3.8"] 15 | 16 | steps: 17 | - uses: actions/checkout@v2 18 | - name: Set up Python ${{ matrix.python-version }} 19 | uses: actions/setup-python@v1 20 | with: 21 | python-version: ${{ matrix.python-version }} 22 | - name: Install dependencies 23 | run: | 24 | python -m pip install --upgrade pip 25 | pip install -e . 26 | - name: Test with pytest 27 | run: | 28 | pip install pytest 29 | pytest 30 | -------------------------------------------------------------------------------- /.github/workflows/lint.yml: -------------------------------------------------------------------------------- 1 | name: lint 2 | 3 | on: 4 | push: 5 | branches: [master] 6 | pull_request: 7 | branches: [master] 8 | 9 | jobs: 10 | lint: 11 | runs-on: ubuntu-latest 12 | strategy: 13 | matrix: 14 | python-version: ["3.7"] 15 | 16 | steps: 17 | - uses: actions/checkout@v2 18 | - name: Set up Python ${{ matrix.python-version }} 19 | uses: actions/setup-python@v1 20 | with: 21 | python-version: ${{ matrix.python-version }} 22 | - name: Install dependencies 23 | run: | 24 | python -m pip install --upgrade pip 25 | pip install -e . 26 | - name: Python style check 27 | uses: bulv1ne/python-style-check@v0.4 28 | -------------------------------------------------------------------------------- /.github/workflows/mypy.yml: -------------------------------------------------------------------------------- 1 | name: mypy 2 | 3 | on: 4 | push: 5 | branches: [master] 6 | pull_request: 7 | branches: [master] 8 | 9 | jobs: 10 | mypy: 11 | runs-on: ubuntu-latest 12 | strategy: 13 | matrix: 14 | python-version: ["3.7"] 15 | 16 | steps: 17 | - uses: actions/checkout@v2 18 | - name: Set up Python ${{ matrix.python-version }} 19 | uses: actions/setup-python@v1 20 | with: 21 | python-version: ${{ matrix.python-version }} 22 | - name: Install dependencies 23 | run: | 24 | python -m pip install --upgrade pip 25 | pip install -e . 26 | pip install mypy 27 | pip install https://github.com/numpy/numpy-stubs/tarball/master 28 | - name: Test with mypy 29 | run: | 30 | mypy . 31 | -------------------------------------------------------------------------------- /.github/workflows/publish.yml: -------------------------------------------------------------------------------- 1 | # This workflows will upload a Python Package using Twine when a release is created 2 | # For more information see: https://help.github.com/en/actions/language-and-framework-guides/using-python-with-github-actions#publishing-to-package-registries 3 | 4 | name: Upload Python Package 5 | 6 | on: 7 | release: 8 | types: [created] 9 | 10 | jobs: 11 | deploy: 12 | 13 | runs-on: ubuntu-latest 14 | 15 | steps: 16 | - uses: actions/checkout@v2 17 | - name: Set up Python 18 | uses: actions/setup-python@v1 19 | with: 20 | python-version: '3.x' 21 | - name: Install dependencies 22 | run: | 23 | python -m pip install --upgrade pip 24 | pip install setuptools wheel twine 25 | - name: Build and publish 26 | env: 27 | TWINE_USERNAME: ${{ secrets.PYPI_USERNAME }} 28 | TWINE_PASSWORD: ${{ secrets.PYPI_PASSWORD }} 29 | run: | 30 | python setup.py sdist bdist_wheel 31 | twine upload dist/* 32 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | *.swp 2 | *.pyc 3 | __pycache__ 4 | docs/build 5 | 6 | # Setuptools distribution folder. 7 | /dist/ 8 | 9 | # Python egg metadata, regenerated from source files by setuptools. 10 | /*.egg-info 11 | -------------------------------------------------------------------------------- /.readthedocs.yml: -------------------------------------------------------------------------------- 1 | build: 2 | image: latest 3 | 4 | python: 5 | version: 3.6 6 | setup_py_install: true 7 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2019 Berkeley Open Arms 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # blue_interface 2 | 3 | [ 4 | **[API Reference](https://blue-interface.readthedocs.io)** 5 | ] [ 6 | **[Examples](https://github.com/berkeleyopenarms/blue_interface/tree/master/examples)** 7 | ] 8 | 9 | 10 | ![robot arm splash](https://brentyi.github.io/filestore/blue_single_arm_resized.png) 11 | 12 | ![build](https://github.com/berkeleyopenarms/blue_interface/workflows/build/badge.svg) 13 | ![mypy](https://github.com/berkeleyopenarms/blue_interface/workflows/mypy/badge.svg?branch=master) 14 | ![lint](https://github.com/berkeleyopenarms/blue_interface/workflows/lint/badge.svg) 15 | 16 | Blue Interface is a platform-agnostic Python API for controlling Blue robotic 17 | arms. It uses [rosbridge](https://github.com/RobotWebTools/rosbridge_suite) to 18 | communicate with Blue's ROS-based control system over a network connection. 19 | 20 | **Features:** 21 | 22 | - No dependency on ROS (or any particular version of Ubuntu) 23 | - Easy connection to multiple robots 24 | - Support for both Python 2 and 3 25 | - Support for Mac, Windows, and Linux 26 | - Support for Jupyter Notebooks 27 | 28 | It's designed to be lightweight and easy-to-use! Sending a Blue "right" arm to 29 | its zero position, for example, is as simple as: 30 | 31 | ```python 32 | from blue_interface import BlueInterface 33 | 34 | blue = BlueInterface(side="right", ip="127.0.0.1") 35 | blue.set_joint_positions([0] * 7) 36 | ``` 37 | 38 | ### Installation 39 | 40 | From PyPi: 41 | 42 | ```sh 43 | pip install blue-interface 44 | ``` 45 | 46 | From source: 47 | 48 | ```sh 49 | git clone https://github.com/berkeleyopenarms/blue_interface.git 50 | cd blue_interface 51 | pip install -e . 52 | ``` 53 | 54 | ### Examples 55 | 56 | - `gripper_controller.py` - An example of opening and closing Blue's gripper. 57 | - `inverse_kinematics.py` - An example of sending Blue an end effector pose 58 | command. 59 | - `print_status.py` - An example of reading state and printing values from Blue. 60 | - `zero_position.py` - An example of sending Blue a command in joint space. 61 | -------------------------------------------------------------------------------- /blue_interface/__init__.py: -------------------------------------------------------------------------------- 1 | from .blue_interface import BlueInterface 2 | 3 | __all__ = ["BlueInterface"] 4 | -------------------------------------------------------------------------------- /blue_interface/blue_interface.py: -------------------------------------------------------------------------------- 1 | import atexit 2 | import threading 3 | import time 4 | from enum import Enum 5 | from typing import Any, Dict, List, Optional, Sequence 6 | 7 | import numpy as np 8 | 9 | from .rosbridge_client import ROSBridgeClient 10 | 11 | 12 | class BlueInterface: 13 | """A Python interface for controlling the Blue robot through rosbridge. 14 | 15 | Args: 16 | side (str): side of the arm, "left" or "right" 17 | ip (str): The IP address of the robot, which by default should have 18 | a running rosbridge server. 19 | port (int, optional): The websocket port number for rosbridge. 20 | Defaults to 9090. 21 | """ 22 | 23 | def __init__( 24 | self, 25 | side, # type: str 26 | ip, # type: str 27 | port=9090, # type: int 28 | ): # type: (...) -> None 29 | assert side == "left" or side == "right" 30 | 31 | self._RBC = ROSBridgeClient(ip, port) 32 | 33 | # ROS topic names 34 | topic_prefix = "/" + side + "_arm/" 35 | ROS_POSITION_TOPIC = ( 36 | topic_prefix + "blue_controllers/joint_position_controller/command" 37 | ) 38 | ROS_SOFT_POSITION_TOPIC = ( 39 | topic_prefix + "blue_controllers/joint_soft_position_controller/command" 40 | ) 41 | ROS_TORQUE_TOPIC = ( 42 | topic_prefix + "blue_controllers/joint_torque_controller/command" 43 | ) 44 | ROS_JOINT_STATE_TOPIC = "/joint_states" 45 | ROS_GRIPPER_TOPIC = ( 46 | topic_prefix + "blue_controllers/gripper_controller/gripper_cmd" 47 | ) 48 | 49 | # Frame names 50 | self._WORLD_FRAME = "base_link" 51 | self._END_EFFECTOR_FRAME = side + "_gripper_finger_link" 52 | 53 | # Joint names 54 | self._joint_names = [ 55 | "{}_{}".format(side, j) 56 | for j in [ 57 | "base_roll_joint", 58 | "shoulder_lift_joint", 59 | "shoulder_roll_joint", 60 | "elbow_lift_joint", 61 | "elbow_roll_joint", 62 | "wrist_lift_joint", 63 | "wrist_roll_joint", 64 | ] 65 | ] 66 | self._gripper_joint_name = side + "_gripper_joint" 67 | 68 | # Controller names 69 | self._controller_lookup = { 70 | _BlueController.GRAV_COMP: "", 71 | _BlueController.POSITION: "blue_controllers/joint_position_controller", 72 | _BlueController.SOFT_POSITION: "blue_controllers/joint_soft_position_controller", 73 | _BlueController.GRIPPER: "blue_controllers/gripper_controller", 74 | _BlueController.TORQUE: "blue_controllers/joint_torque_controller", 75 | } 76 | 77 | # Robot state values! These will be populated later 78 | self._joint_positions = None # type: Optional[np.ndarray] 79 | self._cartesian_pose = None # type: Optional[Dict[str, np.ndarray]] 80 | self._joint_torques = None # type: Optional[np.ndarray] 81 | self._joint_velocities = None # type: Optional[np.ndarray] 82 | self._gripper_goal_id = None # type: Optional[int] 83 | self._gripper_position = None # type: Optional[float] 84 | self._gripper_effort = None # type: Optional[float] 85 | 86 | # Joint state pub/sub 87 | self._joint_state_subscriber = self._RBC.subscriber( 88 | ROS_JOINT_STATE_TOPIC, "sensor_msgs/JointState", self._joint_state_callback 89 | ) 90 | self._joint_position_publisher = self._RBC.publisher( 91 | ROS_POSITION_TOPIC, "std_msgs/Float64MultiArray" 92 | ) 93 | self._joint_soft_position_publisher = self._RBC.publisher( 94 | ROS_SOFT_POSITION_TOPIC, "std_msgs/Float64MultiArray" 95 | ) 96 | self._joint_torque_publisher = self._RBC.publisher( 97 | ROS_TORQUE_TOPIC, "std_msgs/Float64MultiArray" 98 | ) 99 | 100 | # Controller manager services 101 | self._switch_controller_service_client = self._RBC.service( 102 | topic_prefix + "controller_manager/switch_controller", 103 | "controller_manager_msgs/SwitchController", 104 | ) 105 | self._load_controller_service_client = self._RBC.service( 106 | topic_prefix + "controller_manager/load_controller", 107 | "controller_manager_msgs/LoadController", 108 | ) 109 | self._unload_controller_service_client = self._RBC.service( 110 | topic_prefix + "controller_manager/unload_controller", 111 | "controller_manager_msgs/UnloadController", 112 | ) 113 | 114 | # Inverse kinematics service 115 | self._inverse_kinematics_client = self._RBC.service( 116 | topic_prefix + "inverse_kinematics", "blue_msgs/InverseKinematics" 117 | ) 118 | 119 | # Gripper calibration service 120 | self._calibrate_gripper_client = self._RBC.service( 121 | topic_prefix + "calibrate_gripper", "std_srvs/Trigger" 122 | ) 123 | 124 | # TF repub service 125 | self._tf_service_client = self._RBC.service( 126 | "/republish_tfs", "tf2_web_republisher/RepublishTFs" 127 | ) 128 | 129 | # Gripper action client 130 | self._gripper_action_client = self._RBC.action_client( 131 | ROS_GRIPPER_TOPIC, "control_msgs/GripperCommandAction" 132 | ) 133 | 134 | # Start listening to world->end effector transforms 135 | self._request_end_effector_tfs() 136 | 137 | # Cleaner exiting 138 | atexit.register(self.shutdown) 139 | 140 | # Load controllers 141 | self._load_controller(self._controller_lookup[_BlueController.POSITION]) 142 | self._load_controller(self._controller_lookup[_BlueController.SOFT_POSITION]) 143 | self._load_controller(self._controller_lookup[_BlueController.GRIPPER]) 144 | self._load_controller(self._controller_lookup[_BlueController.TORQUE]) 145 | 146 | # Make controllers are stopped 147 | self._switch_controller( 148 | [], 149 | [ 150 | self._controller_lookup[_BlueController.POSITION], 151 | self._controller_lookup[_BlueController.SOFT_POSITION], 152 | self._controller_lookup[_BlueController.GRIPPER], 153 | self._controller_lookup[_BlueController.TORQUE], 154 | ], 155 | ) 156 | self._control_mode = _BlueController.GRAV_COMP 157 | self._gripper_enabled = False 158 | 159 | while self._cartesian_pose is None or self._joint_positions is None: 160 | time.sleep(0.1) 161 | 162 | def shutdown(self): # type: (...) -> None 163 | """Clean up and close connection to host computer. All control will be 164 | disabled. This can be called manually, but will also run automatically 165 | when your script exits.""" 166 | 167 | self._switch_controller( 168 | [], 169 | [ 170 | self._controller_lookup[_BlueController.POSITION], 171 | self._controller_lookup[_BlueController.SOFT_POSITION], 172 | self._controller_lookup[_BlueController.GRIPPER], 173 | self._controller_lookup[_BlueController.TORQUE], 174 | ], 175 | ) 176 | self._unload_controller(self._controller_lookup[_BlueController.POSITION]) 177 | self._unload_controller(self._controller_lookup[_BlueController.SOFT_POSITION]) 178 | self._unload_controller(self._controller_lookup[_BlueController.GRIPPER]) 179 | self._unload_controller(self._controller_lookup[_BlueController.TORQUE]) 180 | self._RBC.close() 181 | 182 | def calibrate_gripper(self): # type: (...) -> None 183 | """Run the gripper position calibration process. 184 | This will automatically determine the gripper position by apply a closing 185 | torque and detecting when the gripper has fully closed.""" 186 | 187 | gripper_enabled = self._gripper_enabled 188 | if gripper_enabled: 189 | self.disable_gripper() 190 | 191 | s = threading.Semaphore(0) 192 | 193 | def callback(success, values): 194 | s.release() 195 | 196 | self._calibrate_gripper_client.request({}, callback) 197 | s.acquire() 198 | 199 | if gripper_enabled: 200 | self.enable_gripper() 201 | 202 | def command_gripper( 203 | self, 204 | position, # type: float 205 | effort, # type: float 206 | wait=False, # type: bool 207 | ): # type: (...) -> None 208 | """Send a goal to gripper, and optionally wait for the goal to be reached. 209 | 210 | Args: 211 | position (float64): gap size between gripper fingers in cm. 212 | effort (float64): maximum effort the gripper with exert before 213 | stalling in N. 214 | """ 215 | # TODO: change robot-side so position and effort in correct units 216 | 217 | if not self._gripper_enabled: 218 | self.enable_gripper() 219 | 220 | goal_msg = {"command": {"position": position, "max_effort": effort}} 221 | 222 | s = threading.Semaphore(0) 223 | 224 | def callback(result, status): 225 | if result["stalled"] or result["reached_goal"]: 226 | s.release() 227 | 228 | self._gripper_goal_id = self._gripper_action_client.send_goal( 229 | goal_msg, callback, callback 230 | ) 231 | if wait: 232 | s.acquire() 233 | 234 | def cancel_gripper_command(self): # type: (...) -> None 235 | """Cancel current gripper command, halting gripper in current position.""" 236 | self._gripper_action_client.cancel_goal(self._gripper_goal_id) 237 | 238 | def get_gripper_position(self): # type: (...) -> float 239 | """Get the current gap between gripper fingers. 240 | 241 | Returns: 242 | float64: the gripper gap in cm. 243 | 244 | """ 245 | assert self._gripper_position is not None, "Gripper position not yet populated!" 246 | return self._gripper_position 247 | 248 | def get_gripper_effort(self): # type: (...) -> float 249 | """Get the current effort exerted by the gripper. 250 | 251 | Returns: 252 | float64: the gripper effort in N 253 | """ 254 | assert self._gripper_effort is not None, "Gripper effort not yet populated!" 255 | return self._gripper_effort 256 | 257 | def set_joint_positions( 258 | self, 259 | joint_positions, # type: Sequence 260 | duration=0.0, # type: float 261 | soft_position_control=False, # type: bool 262 | ): # type: (...) -> None 263 | """Move arm to specified position in joint space. 264 | 265 | Args: 266 | joint_positions (iterable): An array of 7 joint angles, in radians, 267 | ordered from proximal to distal. 268 | duration (float, optional): Seconds to take to reach the target, 269 | interpolating in joint space. Defaults to 0. 270 | soft_position_control (bool, optional): Use "software" position 271 | control, which runs position control loop at the ROS-level, 272 | rather than on the motor drivers. This should be rarely needed. 273 | Defaults to False. 274 | """ 275 | joint_positions = np.asarray(joint_positions) 276 | assert len(joint_positions) == 7 277 | 278 | self._set_control_mode( 279 | _BlueController.SOFT_POSITION 280 | if soft_position_control 281 | else _BlueController.POSITION 282 | ) 283 | 284 | start_positions = self.get_joint_positions() 285 | start_time = time.time() 286 | end_time = start_time + duration 287 | while time.time() < end_time: 288 | scale = (time.time() - start_time) / duration 289 | self._set_joint_positions( 290 | start_positions + scale * (joint_positions - start_positions), 291 | soft_position_control, 292 | ) 293 | time.sleep(1.0 / 60.0) 294 | 295 | self._set_joint_positions(joint_positions, soft_position_control) 296 | 297 | def _set_joint_positions( 298 | self, 299 | joint_positions, # type: Sequence 300 | soft_position_control, # type: bool 301 | ): # type: (...) -> None 302 | joint_positions_msg = {"layout": {}, "data": list(joint_positions)} 303 | if soft_position_control: 304 | self._joint_soft_position_publisher.publish(joint_positions_msg) 305 | else: 306 | self._joint_position_publisher.publish(joint_positions_msg) 307 | 308 | def set_joint_torques( 309 | self, joint_torques # type: Sequence 310 | ): # type: (...) -> None 311 | """Command joint torques to the arm. 312 | 313 | Args: 314 | joint_torques (iterable): An array of 7 joint torques, in Nm, 315 | ordered from proximal to distal. 316 | """ 317 | 318 | joint_torques = list(joint_torques) 319 | assert len(joint_torques) == 7 320 | 321 | self._set_control_mode(_BlueController.TORQUE) 322 | 323 | joint_torques_msg = {"layout": {}, "data": list(joint_torques)} 324 | self._joint_torque_publisher.publish(joint_torques_msg) 325 | 326 | def get_joint_positions(self): # type: (...) -> np.ndarray 327 | """Get the current joint angles, in radians. 328 | 329 | Returns: 330 | numpy.ndarray: An array of 7 angles, in radians, ordered from 331 | proximal to distal. 332 | """ 333 | assert self._joint_positions is not None, "Joint positions not populated!" 334 | return self._joint_positions 335 | 336 | def get_cartesian_pose(self): # type: (...) -> Dict[str, np.ndarray] 337 | """Get the current cartesian pose of the end effector, with respect to 338 | the world frame. 339 | 340 | Returns: 341 | dict: Pose in the form {"position": numpy.array([x,y,z]), 342 | "orientation": numpy.array([x,y,z,w]} defined with respect to the 343 | world frame. 344 | """ 345 | assert self._cartesian_pose is not None, "Cartesian pose not populated!" 346 | return self._cartesian_pose 347 | 348 | def get_joint_torques(self): # type: (...) -> np.ndarray 349 | """Get the current joint torques. 350 | 351 | Returns: 352 | numpy.ndarray: An array of 7 joint torques, in Nm, ordered from 353 | proximal to distal. 354 | """ 355 | assert self._joint_torques is not None, "Joint torques not populated!" 356 | return self._joint_torques 357 | 358 | def get_joint_velocities(self): # type: (...) -> np.ndarray 359 | """Get the current joint velocities. 360 | 361 | Returns: 362 | numpy.ndarray: An array of 7 joint torques, in Nm, ordered from 363 | proximal to distal. 364 | """ 365 | assert self._joint_velocities is not None, "Joint velocities not populated!" 366 | return self._joint_velocities 367 | 368 | def disable_control(self): # type: (...) -> None 369 | """Set joint control mode to gravity compensation only.""" 370 | self._set_control_mode(_BlueController.GRAV_COMP) 371 | 372 | def enable_gripper(self): # type: (...) -> None 373 | """Enables the gripper. The gripper will begin to hold position.""" 374 | self._switch_controller([self._controller_lookup[_BlueController.GRIPPER]], []) 375 | self._gripper_enabled = True 376 | 377 | def disable_gripper(self): # type: (...) -> None 378 | """Disables the gripper. The gripper will become compliant.""" 379 | self._switch_controller([], [self._controller_lookup[_BlueController.GRIPPER]]) 380 | self._gripper_enabled = False 381 | 382 | def gripper_enabled(self): # type: (...) -> bool 383 | """Check if gripper is enabled to take commands. 384 | 385 | Returns: 386 | bool: True if enabled, False otherwise. 387 | """ 388 | return self._gripper_enabled 389 | 390 | def inverse_kinematics( 391 | self, 392 | position, # type: np.ndarray 393 | orientation, # type: np.ndarray 394 | seed_joint_positions=[], # type: Sequence 395 | ): # type: (...) -> np.ndarray 396 | """Given a desired cartesian pose for the end effector, compute the 397 | necessary joint angles. Note that the system is underparameterized and 398 | there are an infinite number of possible solutions; this will only 399 | return a single possible one. 400 | 401 | Args: 402 | position (iterable): A length-3 array containing a cartesian position 403 | (x,y,z), wrt the world frame. 404 | orientation (iterable): A length-4 array containing a quaternion 405 | (x,y,z,w), wrt the world frame. 406 | seed_joint_positions (iterable, optional): An array of 7 joint 407 | angles, to be used to initalize the IK solver. 408 | Returns: 409 | numpy.ndarray: An array of 7 joint angles, or an empty array if no 410 | solution was found. 411 | """ 412 | 413 | output = [] 414 | s = threading.Semaphore(0) 415 | 416 | def callback(success, values): 417 | if success: 418 | output.extend(values["ik_joint_positions"]) 419 | s.release() 420 | 421 | request_msg = { 422 | "end_effector_pose": { 423 | "header": {"frame_id": self._WORLD_FRAME}, 424 | "pose": { 425 | "position": dict(zip("xyz", position)), 426 | "orientation": dict(zip("xyzw", orientation)), 427 | }, 428 | }, 429 | "solver": "trac-ik", 430 | "seed_joint_positions": seed_joint_positions, 431 | } 432 | self._inverse_kinematics_client.request(request_msg, callback) 433 | s.acquire() 434 | 435 | return np.asarray(output) 436 | 437 | def _joint_state_callback( 438 | self, message # type: Dict[str, Any] 439 | ): # type: (...) -> None 440 | joint_positions_temp = [] 441 | joint_torques_temp = [] 442 | joint_velocities_temp = [] 443 | 444 | for name in self._joint_names: 445 | if name not in message["name"]: 446 | continue 447 | else: 448 | self.index = message["name"].index(name) 449 | joint_positions_temp.append(message["position"][self.index]) 450 | joint_torques_temp.append(message["effort"][self.index]) 451 | joint_velocities_temp.append(message["velocity"][self.index]) 452 | 453 | if self._gripper_joint_name in message["name"]: 454 | index = message["name"].index(self._gripper_joint_name) 455 | self._gripper_position = message["position"][index] 456 | self._gripper_effort = message["effort"][index] 457 | 458 | if len(joint_positions_temp) != 0: 459 | self._joint_positions = np.array(joint_positions_temp) 460 | if len(joint_torques_temp) != 0: 461 | self._joint_torques = np.array(joint_torques_temp) 462 | if len(joint_velocities_temp) != 0: 463 | self._joint_velocities = np.array(joint_velocities_temp) 464 | 465 | def _process_tfs( 466 | self, message # type: Dict[str, Any] 467 | ): # type: (...) -> None 468 | pose = message["transforms"][0]["transform"] 469 | trans = pose["translation"] 470 | rot = pose["rotation"] 471 | cartesian_pose_temp = {} 472 | cartesian_pose_temp["position"] = np.array([trans["x"], trans["y"], trans["z"]]) 473 | cartesian_pose_temp["orientation"] = np.array( 474 | [rot["x"], rot["y"], rot["z"], rot["w"]] 475 | ) 476 | self._cartesian_pose = cartesian_pose_temp 477 | 478 | def _request_end_effector_tfs(self): # type: (...) -> None 479 | goal_msg = { 480 | "source_frames": [self._END_EFFECTOR_FRAME], 481 | "target_frame": self._WORLD_FRAME, 482 | "angular_thres": 0, 483 | "trans_thres": 0, 484 | "rate": 30, 485 | "timeout": {"secs": 2.0, "nsecs": 0.0}, 486 | } 487 | 488 | def _tf_service_callback(success, values): 489 | if success: 490 | self._tf_subscriber = self._RBC.subscriber( 491 | values["topic_name"], 492 | "tf2_web_republisher/TFArray", 493 | self._process_tfs, 494 | ) 495 | 496 | self._tf_service_client.request(goal_msg, _tf_service_callback) 497 | 498 | def _set_control_mode( 499 | self, mode # type: _BlueController 500 | ): # type: (...) -> bool 501 | if mode == self._control_mode: 502 | return True 503 | self._switch_controller( 504 | [self._controller_lookup[mode]], 505 | [self._controller_lookup[self._control_mode]], 506 | new_control_mode=mode, 507 | ) 508 | return mode == self._control_mode 509 | 510 | def _switch_controller( 511 | self, 512 | start, # type: List[str] 513 | stop, # type: List[str] 514 | new_control_mode=None, # type: Optional[_BlueController] 515 | ): # type: (...) -> None 516 | request_msg = { 517 | "start_controllers": start, 518 | "stop_controllers": stop, 519 | "strictness": 1, # best effort 520 | } 521 | 522 | s = threading.Semaphore(0) 523 | 524 | def callback(success, values): 525 | if success and new_control_mode is not None: 526 | self._control_mode = new_control_mode 527 | s.release() 528 | 529 | self._switch_controller_service_client.request(request_msg, callback) 530 | s.acquire() 531 | 532 | # Even after the controller is successfully switched, it needs a moment 533 | # to instantiate the command topic subscriber, etc 534 | time.sleep(0.01) 535 | 536 | def _load_controller( 537 | self, name # type: str 538 | ): # type: (...) -> None 539 | request_msg = {"name": name} 540 | 541 | s = threading.Semaphore(0) 542 | 543 | def callback(success, values): 544 | s.release() 545 | 546 | self._load_controller_service_client.request(request_msg, callback) 547 | s.acquire() 548 | 549 | def _unload_controller( 550 | self, name # type: str 551 | ): # type: (...) -> None 552 | request_msg = {"name": name} 553 | 554 | s = threading.Semaphore(0) 555 | 556 | def callback(success, values): 557 | s.release() 558 | 559 | self._unload_controller_service_client.request(request_msg, callback) 560 | s.acquire() 561 | 562 | 563 | class _BlueController(Enum): 564 | """Enum for specifying Blue controller types. 565 | Note that GRAV_COMP, POSITION, and TORQUE are mutually exclusive. 566 | 567 | Attributes: 568 | GRAV_COMP: No joint control; pure gravity compensation 569 | POSITION: Joint position controller 570 | SOFT_POSITION: "Soft" joint position controller 571 | This runs control at the ROS-level at ~140Hz 572 | instead of on the motor drivers at ~20kHz -- 573 | control is worse but allows gains to be tuned 574 | dynamically 575 | TORQUE: Joint torque controller 576 | GRIPPER: Gripper controller 577 | """ 578 | 579 | GRAV_COMP = 0 580 | POSITION = 1 581 | SOFT_POSITION = 2 582 | TORQUE = 3 583 | GRIPPER = 4 584 | -------------------------------------------------------------------------------- /blue_interface/rosbridge_client.py: -------------------------------------------------------------------------------- 1 | """This module provides a python client for rosbridge to publish, subscribe topics, 2 | call services and use action client. 3 | """ 4 | 5 | # source: http://codegists.com/snippet/python/rosbridge_clientpy_le-kang_python 6 | # TODO: look into replacing this with something like 7 | # https://github.com/gramaziokohler/roslibpy 8 | 9 | import json 10 | import threading 11 | import time 12 | import uuid 13 | 14 | from pydispatch import dispatcher 15 | from ws4py.client.threadedclient import WebSocketClient 16 | 17 | 18 | class ROSBridgeClient(WebSocketClient): 19 | """ROSBridgeClient extends WebSocketClient and manages connection to the server and all interactions with ROS. 20 | 21 | It keeps a record of all publishers, subscriber, service request callbacks and action clients. 22 | """ 23 | 24 | def __init__(self, ip, port=9090): 25 | """Constructor for ROSBridgeClient 26 | 27 | Args: 28 | ip (str): The robot IP address. 29 | port (int, optional): The WebSocket port number for rosbridge. Defaults to 9090. 30 | """ 31 | WebSocketClient.__init__(self, "ws://{}:{}".format(ip, port)) 32 | self._connected = False 33 | self._id_counter = 0 34 | self._publishers = {} 35 | self._subscribers = {} 36 | self._services = {} 37 | self._action_clients = {} 38 | self.connect() 39 | th = threading.Thread(target=self.run_forever) 40 | th.daemon = True 41 | th.start() 42 | while not self._connected: 43 | time.sleep(0.1) 44 | 45 | @property 46 | def id_counter(self): 47 | """Generate an auto-incremental ID starts from 1. 48 | 49 | Returns: 50 | A auto-incremented ID. 51 | """ 52 | self._id_counter += 1 53 | return self._id_counter 54 | 55 | def publisher(self, topic_name, message_type, latch=False, queue_size=1): 56 | """Create a _Publisher object if the given topic hasn't been advertised, otherwise return the existing 57 | publisher that is currently advertising the topic. 58 | 59 | Args: 60 | topic_name (str): The ROS topic name. 61 | message_type (str): The ROS message type, such as `std_msgs/String`. 62 | latch (bool, optional): Whether the topic is latched when publishing. Defaults to False. 63 | queue_size (int): The queue created at bridge side for re-publishing. Defaults to 1. 64 | 65 | Returns: 66 | A _Publisher object. 67 | """ 68 | if topic_name in self._publishers: 69 | publisher = self._publishers.get(topic_name) 70 | publisher.usage += 1 71 | else: 72 | print("Advertising topic {} for publishing".format(topic_name)) 73 | publisher = _Publisher(self, topic_name, message_type, latch, queue_size) 74 | self._publishers[topic_name] = publisher 75 | return publisher 76 | 77 | def unregister_publisher(self, topic_name): 78 | """Stop advertising on the given topic. 79 | 80 | Args: 81 | topic_name (str): The ROS topic name. 82 | """ 83 | if topic_name in self._publishers: 84 | print("Stop advertising topic {} for publishing".format(topic_name)) 85 | del self._publishers[topic_name] 86 | 87 | def subscriber(self, topic_name, message_type, cb): 88 | """Create a _Subscriber object on a given topic with a callback function. 89 | 90 | If the topic hasn't been subscribed yet, subscribe the topic. Otherwise, it adds the subscriber 91 | with callback function into the topic subscription list. 92 | 93 | Args: 94 | topic_name (str): The ROS topic name. 95 | message_type (str): The ROS message type, such as `std_msgs/String`. 96 | cb (function): A function will be called when a message is received on that topic. 97 | 98 | Returns: 99 | A _Subscriber object. 100 | """ 101 | subscriber = _Subscriber(self, topic_name, cb) 102 | if topic_name in self._subscribers: 103 | self._subscribers.get(topic_name).get("subscribers").append(subscriber) 104 | else: 105 | subscribe_id = "subscribe:{}:{}".format(topic_name, self._id_counter) 106 | print("Sending request to subscribe topic {}".format(topic_name)) 107 | self.send( 108 | json.dumps( 109 | { 110 | "op": "subscribe", 111 | "id": subscribe_id, 112 | "topic": topic_name, 113 | "type": message_type, 114 | } 115 | ) 116 | ) 117 | self._subscribers[topic_name] = {} 118 | self._subscribers[topic_name]["subscribe_id"] = subscribe_id 119 | self._subscribers[topic_name]["subscribers"] = [subscriber] 120 | return subscriber 121 | 122 | def unsubscribe(self, subscriber): 123 | """Remove a callback subscriber from its topic subscription list. 124 | 125 | If there is no callback subscribers in the subscription list. It will unsubscribe the topic. 126 | 127 | Args: 128 | subscriber (_Subscriber): A subscriber with callback function that listen to the topic. 129 | """ 130 | topic_name = subscriber.topic_name 131 | if topic_name not in self._subscribers: 132 | return 133 | subscribe_id = self._subscribers.get(topic_name).get("subscribe_id") 134 | subscribers = self._subscribers.get(topic_name).get("subscribers") 135 | if subscriber in subscribers: 136 | subscribers.remove(subscriber) 137 | if len(subscribers) == 0: 138 | print("Sending request to unsubscribe topic {}".format(topic_name)) 139 | del subscribers[:] 140 | self.send( 141 | json.dumps( 142 | {"op": "unsubscribe", "id": subscribe_id, "topic": topic_name} 143 | ) 144 | ) 145 | del self._subscribers[topic_name] 146 | 147 | def service(self, service_name, service_type): 148 | """Create a ROS service client. 149 | 150 | Args: 151 | service_name (str): The ROS service name. 152 | service_type (str): The ROS service type. 153 | 154 | Returns: 155 | A _Service object. 156 | """ 157 | return _Service(self, service_name, service_type) 158 | 159 | def register_service_callback(self, service_id, cb): 160 | """Register a service callback with a service request ID. 161 | 162 | Args: 163 | service_id (str): The service request ID. 164 | cb (function): A function will be called when the service server responses. 165 | """ 166 | self._services[service_id] = cb 167 | 168 | def action_client(self, server_name, action_name): 169 | """Create a ROS action client if there was no client created for the action server. 170 | Otherwise return that action client. 171 | 172 | Args: 173 | server_name (str): The ROS action server name. 174 | action_name (str): The ROS action name. 175 | 176 | Returns: 177 | A _ActionClient object. 178 | """ 179 | if server_name + ":" + action_name in self._action_clients: 180 | action_client = self._action_clients.get( 181 | server_name + ":" + action_name 182 | ).get("action_client") 183 | action_client.usage += 1 184 | else: 185 | action_client = _ActionClient(self, server_name, action_name) 186 | return action_client 187 | 188 | def unregister_action_client(self, server_name, action_name): 189 | """Unregister the action client with server and action name. Remove it from the internal action client dict. 190 | 191 | Args: 192 | server_name (str): The ROS action server name. 193 | action_name (str): The ROS action name. 194 | """ 195 | if server_name + ":" + action_name in self._action_clients: 196 | del self._action_clients[server_name + ":" + action_name] 197 | 198 | def opened(self): 199 | """Called when the connection to ROS established.""" 200 | self._connected = True 201 | print("Connected with rosbridge") 202 | 203 | def closed(self, code, reason=None): 204 | """Called when the connection to ROS disconnected 205 | 206 | Args: 207 | code (int): A status code. 208 | reason (str, opitonal): A human readable message. Defaults to None. 209 | """ 210 | print("Disconnected with rosbridge") 211 | 212 | def received_message(self, message): 213 | """Called when message received from ROS server. 214 | 215 | Only handle the message with `topic` or `service` keywords and trigger corresponding callback functions. 216 | 217 | Args: 218 | message(ws4py.messaging.Message): A message that sent from ROS server. 219 | """ 220 | message.data = message.data.decode() 221 | data = json.loads(message.data) 222 | if "topic" in data: 223 | dispatcher.send(data.get("topic"), message=data.get("msg")) 224 | if "service" in data: 225 | service_id = data.get("id") 226 | success = data.get("result") 227 | values = data.get("values") 228 | if service_id in self._services: 229 | self._services[service_id](success, values) 230 | del self._services[service_id] 231 | 232 | def unhandled_error(self, error): 233 | """Called when a socket or OS error is raised. 234 | 235 | Args: 236 | error (str): A human readable error message. 237 | """ 238 | print(error) 239 | 240 | 241 | class _Publisher(object): 242 | def __init__(self, rosbridge, topic_name, message_type, latch=False, queue_size=1): 243 | """Constructor for _Publisher. 244 | 245 | Args: 246 | rosbridge (ROSBridgeClient): The ROSBridgeClient object. 247 | topic_name (str): The ROS topic name. 248 | message_type (str): The ROS message type, such as `std_msgs/String`. 249 | latch (bool, optional): Whether the topic is latched when publishing. Defaults to False. 250 | queue_size (int): The queue created at bridge side for re-publishing. Defaults to 1. 251 | """ 252 | self._advertise_id = "advertise:{}:{}".format(topic_name, rosbridge.id_counter) 253 | self._rosbridge = rosbridge 254 | self._topic_name = topic_name 255 | self._usage = 1 256 | 257 | rosbridge.send( 258 | json.dumps( 259 | { 260 | "op": "advertise", 261 | "id": self._advertise_id, 262 | "topic": topic_name, 263 | "type": message_type, 264 | "latch": latch, 265 | "queue_size": queue_size, 266 | } 267 | ) 268 | ) 269 | 270 | @property 271 | def usage(self): 272 | return self._usage 273 | 274 | @usage.setter 275 | def usage(self, value): 276 | self._usage = value 277 | 278 | def publish(self, message): 279 | """Publish a ROS message 280 | 281 | Args: 282 | message (dict): A message to send. 283 | """ 284 | self._rosbridge.send( 285 | json.dumps( 286 | { 287 | "op": "publish", 288 | "id": "publish:{}:{}".format( 289 | self._topic_name, self._rosbridge.id_counter 290 | ), 291 | "topic": self._topic_name, 292 | "msg": message, 293 | } 294 | ) 295 | ) 296 | 297 | def unregister(self): 298 | """Reduce the usage of the publisher. If the usage is 0, unadvertise this topic.""" 299 | self._usage -= 1 300 | if self._usage == 0: 301 | self._rosbridge.unregister_publisher(self._topic_name) 302 | self._rosbridge.send( 303 | json.dumps( 304 | { 305 | "op": "unadvertise", 306 | "id": self._advertise_id, 307 | "topic": self._topic_name, 308 | } 309 | ) 310 | ) 311 | 312 | 313 | class _Subscriber(object): 314 | def __init__(self, rosbridge, topic_name, cb=None): 315 | """Constructor for _Subscriber. 316 | 317 | Args: 318 | rosbridge (ROSBridgeClient): The ROSBridgeClient object. 319 | topic_name (str): The ROS topic name. 320 | cb (function): A function will be called when a message is received on that topic. 321 | """ 322 | self._rosbridge = rosbridge 323 | self._topic_name = topic_name 324 | self._cb = cb 325 | if callable(self._cb): 326 | dispatcher.connect(self._cb, signal=topic_name) 327 | 328 | @property 329 | def topic_name(self): 330 | return self._topic_name 331 | 332 | def unregister(self): 333 | """Remove the current callback function from listening to the topic, 334 | and from the rosbridge client subscription list 335 | """ 336 | if callable(self._cb): 337 | dispatcher.disconnect(self._cb, signal=self._topic_name) 338 | self._rosbridge.unsubscribe(self) 339 | 340 | 341 | class _Service(object): 342 | def __init__(self, rosbridge, service_name, service_type): 343 | """Constructor for _Service. 344 | 345 | Args: 346 | rosbridge (ROSBridgeClient): The ROSBridgeClient object. 347 | service_name (str): The ROS service name. 348 | service_type (str): The ROS service type. 349 | """ 350 | self._rosbridge = rosbridge 351 | self._service_name = service_name 352 | self._service_type = service_type 353 | 354 | def request(self, request, cb): 355 | """Send a request to the ROS service server. The callback function will be called when service responses. 356 | 357 | Args: 358 | request (dict): A request message to send, 359 | cb (function): A function will be called when the service server responses. 360 | 361 | Returns: 362 | 363 | """ 364 | service_id = "call_service:{}:{}".format( 365 | self._service_name, self._rosbridge.id_counter 366 | ) 367 | if callable(cb): 368 | self._rosbridge.register_service_callback(service_id, cb) 369 | self._rosbridge.send( 370 | json.dumps( 371 | { 372 | "op": "call_service", 373 | "id": service_id, 374 | "service": self._service_name, 375 | "args": request, 376 | } 377 | ) 378 | ) 379 | 380 | 381 | class _ActionClient(object): 382 | def __init__(self, rosbridge, server_name, action_name): 383 | """Constructor for _ActionClient 384 | 385 | Args: 386 | rosbridge (ROSBridgeClient): The ROSBridgeClient object. 387 | server_name (str): The ROS action server name. 388 | action_name (str): The ROS action name. 389 | """ 390 | self._rosbridge = rosbridge 391 | self._server_name = server_name 392 | self._action_name = action_name 393 | self._goals = {} 394 | self._usage = 1 395 | 396 | self._feedback_sub = rosbridge.subscriber( 397 | server_name + "/feedback", action_name + "Feedback", self.on_feedback 398 | ) 399 | self._result_sub = rosbridge.subscriber( 400 | server_name + "/result", action_name + "Result", self.on_result 401 | ) 402 | 403 | self._goal_pub = rosbridge.publisher( 404 | server_name + "/goal", action_name + "Goal" 405 | ) 406 | self._cancel_pub = rosbridge.publisher( 407 | server_name + "/cancel", "actionlib_msgs/GoalID" 408 | ) 409 | 410 | @property 411 | def usage(self): 412 | return self._usage 413 | 414 | @usage.setter 415 | def usage(self, value): 416 | self._usage = value 417 | 418 | def on_feedback(self, message): 419 | """Callback when a feedback message received. 420 | 421 | Args: 422 | message (dict): A feedback message received from ROS action server. 423 | """ 424 | goal = self._goals.get(message.get("status").get("goal_id").get("id")) 425 | if goal: 426 | goal.feedback_received(message.get("feedback"), message.get("status")) 427 | 428 | def on_result(self, message): 429 | """Callback when a result message received. 430 | 431 | Args: 432 | message (dict): A result message received from ROS action server. 433 | """ 434 | goal = self._goals.get(message.get("status").get("goal_id").get("id")) 435 | if goal: 436 | goal.result_received(message.get("result"), message.get("status")) 437 | 438 | def send_goal(self, goal_message, on_result, on_feedback): 439 | """Send a goal to the ROS action server. 440 | 441 | Args: 442 | goal_message (dict): A message to send to ROS action server. 443 | on_result (function): A callback function to be called when a feedback message received. 444 | on_feedback (function): A callback function to be called when a result message received. 445 | """ 446 | goal = _Goal(goal_message, on_result, on_feedback) 447 | self._goals[goal.id] = goal 448 | self._goal_pub.publish(goal.message) 449 | return goal.id 450 | 451 | def cancel_goal(self, goal_id): 452 | """Cancel a goal with a given goal ID 453 | 454 | Args: 455 | goal_id (str): The ID of the goal to be cancelled. 456 | """ 457 | self._cancel_pub.publish({"id": goal_id}) 458 | 459 | def unregister(self): 460 | """Reduce the usage of the action client. If the usage is 0, unregister its publishers and subscribers.""" 461 | self._usage -= 1 462 | if self._usage == 0: 463 | self._feedback_sub.unregister() 464 | self._result_sub.unregister() 465 | self._goal_pub.unregister() 466 | self._cancel_pub.unregister() 467 | self._rosbridge.unregister_action_client( 468 | self._server_name, self._action_name 469 | ) 470 | 471 | 472 | class _Goal(object): 473 | def __init__(self, message, on_result=None, on_feedback=None): 474 | """Constructor for _Goal 475 | 476 | Args: 477 | message (dict): The goal message to send to ROS action server. 478 | on_result (function): A callback function to be called when a feedback message received. 479 | on_feedback (function): A callback function to be called when a result message received. 480 | """ 481 | self._id = "goal_" + str(uuid.uuid4()) 482 | self._message = message 483 | self._is_finished = False 484 | self._on_result = on_result 485 | self._on_feedback = on_feedback 486 | 487 | @property 488 | def id(self): 489 | return self._id 490 | 491 | @property 492 | def message(self): 493 | """Wrap message in JSON format that complies ROSBridge protocol. 494 | 495 | Returns: 496 | A Json that contains the goal ID and message. 497 | """ 498 | return { 499 | "goal_id": {"stamp": {"secs": 0, "nsecs": 0}, "id": self._id}, 500 | "goal": self._message, 501 | } 502 | 503 | @property 504 | def is_finished(self): 505 | return self._is_finished 506 | 507 | def result_received(self, result, status): 508 | """Called when a result message is received. 509 | 510 | Args: 511 | result (dict): The result message. 512 | status (int): The status code. Such as: 513 | ACTIVE = 1: The goal is currently being processed by the action server; 514 | PREEMPTED = 2: The goal received a cancel request after it started executing; 515 | SUCCEEDED = 3: The goal was achieved successfully by the action server; 516 | ABORTED = 4: The goal was aborted during execution by the action server due to some failure. 517 | For more details, refer to http://docs.ros.org/indigo/api/actionlib_msgs/html/msg/GoalStatus.html. 518 | """ 519 | self._is_finished = True 520 | if callable(self._on_result): 521 | self._on_result(result, status) 522 | 523 | def feedback_received(self, feedback, status): 524 | """Called when a result message is received. 525 | 526 | Args: 527 | feedback (dict): The feedback message. 528 | status (int): The status code. Such as: 529 | ACTIVE = 1: The goal is currently being processed by the action server; 530 | PREEMPTED = 2: The goal received a cancel request after it started executing; 531 | SUCCEEDED = 3: The goal was achieved successfully by the action server; 532 | ABORTED = 4: The goal was aborted during execution by the action server due to some failure. 533 | For more details, refer to http://docs.ros.org/indigo/api/actionlib_msgs/html/msg/GoalStatus.html. 534 | """ 535 | if callable(self._on_feedback): 536 | self._on_feedback(feedback, status) 537 | -------------------------------------------------------------------------------- /docs/.gitignore: -------------------------------------------------------------------------------- 1 | build/ 2 | -------------------------------------------------------------------------------- /docs/Makefile: -------------------------------------------------------------------------------- 1 | # Minimal makefile for Sphinx documentation 2 | # 3 | 4 | # You can set these variables from the command line. 5 | SPHINXOPTS = 6 | SPHINXBUILD = sphinx-build 7 | SPHINXPROJ = BlueInterface 8 | SOURCEDIR = source 9 | BUILDDIR = ./build 10 | 11 | # Put it first so that "make" without argument is like "make help". 12 | help: 13 | @$(SPHINXBUILD) -M help "$(SOURCEDIR)" "$(BUILDDIR)" $(SPHINXOPTS) $(O) 14 | 15 | .PHONY: help Makefile 16 | 17 | # Catch-all target: route all unknown targets to Sphinx using the new 18 | # "make mode" option. $(O) is meant as a shortcut for $(SPHINXOPTS). 19 | %: Makefile 20 | @$(SPHINXBUILD) -M $@ "$(SOURCEDIR)" "$(BUILDDIR)" $(SPHINXOPTS) $(O) 21 | -------------------------------------------------------------------------------- /docs/_config.yml: -------------------------------------------------------------------------------- 1 | theme: jekyll-theme-slate 2 | -------------------------------------------------------------------------------- /docs/source/conf.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | # 3 | # Configuration file for the Sphinx documentation builder. 4 | # 5 | # This file does only contain a selection of the most common options. For a 6 | # full list see the documentation: 7 | # http://www.sphinx-doc.org/en/stable/config 8 | 9 | # -- Path setup -------------------------------------------------------------- 10 | 11 | # If extensions (or modules to document with autodoc) are in another directory, 12 | # add these directories to sys.path here. If the directory is relative to the 13 | # documentation root, use os.path.abspath to make it absolute, like shown here. 14 | # 15 | import os 16 | import sys 17 | 18 | sys.path.insert(0, os.path.abspath("../")) 19 | 20 | # -- Project information ----------------------------------------------------- 21 | 22 | project = "Blue Interface" 23 | copyright = "2019" 24 | author = "Rachel Thomasson, Brent Yi, Greg Balke, Philipp Wu" 25 | 26 | # The short X.Y version 27 | version = "" 28 | # The full version, including alpha/beta/rc tags 29 | release = "" 30 | 31 | 32 | # -- General configuration --------------------------------------------------- 33 | 34 | # If your documentation needs a minimal Sphinx version, state it here. 35 | # 36 | # needs_sphinx = '1.0' 37 | 38 | # Add any Sphinx extension module names here, as strings. They can be 39 | # extensions coming with Sphinx (named 'sphinx.ext.*') or your custom 40 | # ones. 41 | extensions = [ 42 | "sphinx.ext.autodoc", 43 | "sphinx.ext.todo", 44 | "sphinx.ext.coverage", 45 | "sphinx.ext.mathjax", 46 | "sphinx.ext.githubpages", 47 | "sphinx.ext.napoleon", 48 | # 'sphinx.ext.viewcode', 49 | ] 50 | 51 | # Add any paths that contain templates here, relative to this directory. 52 | templates_path = ["_templates"] 53 | 54 | # The suffix(es) of source filenames. 55 | # You can specify multiple suffix as a list of string: 56 | # 57 | # source_suffix = ['.rst', '.md'] 58 | source_suffix = ".rst" 59 | 60 | # The master toctree document. 61 | master_doc = "index" 62 | 63 | # The language for content autogenerated by Sphinx. Refer to documentation 64 | # for a list of supported languages. 65 | # 66 | # This is also used if you do content translation via gettext catalogs. 67 | # Usually you set "language" from the command line for these cases. 68 | language = None 69 | 70 | # List of patterns, relative to source directory, that match files and 71 | # directories to ignore when looking for source files. 72 | # This pattern also affects html_static_path and html_extra_path . 73 | exclude_patterns = [] 74 | 75 | # The name of the Pygments (syntax highlighting) style to use. 76 | pygments_style = "sphinx" 77 | 78 | 79 | # -- Options for HTML output ------------------------------------------------- 80 | 81 | # The theme to use for HTML and HTML Help pages. See the documentation for 82 | # a list of builtin themes. 83 | # 84 | html_theme = "sphinx_rtd_theme" 85 | 86 | # Theme options are theme-specific and customize the look and feel of a theme 87 | # further. For a list of options available for each theme, see the 88 | # documentation. 89 | # 90 | # html_theme_options = {} 91 | 92 | # Add any paths that contain custom static files (such as style sheets) here, 93 | # relative to this directory. They are copied after the builtin static files, 94 | # so a file named "default.css" will overwrite the builtin "default.css". 95 | html_static_path = ["_static"] 96 | 97 | # Custom sidebar templates, must be a dictionary that maps document names 98 | # to template names. 99 | # 100 | # The default sidebars (for documents that don't match any pattern) are 101 | # defined by theme itself. Builtin themes are using these templates by 102 | # default: ``['localtoc.html', 'relations.html', 'sourcelink.html', 103 | # 'searchbox.html']``. 104 | # 105 | # html_sidebars = {} 106 | 107 | 108 | # -- Options for HTMLHelp output --------------------------------------------- 109 | 110 | # Output file base name for HTML help builder. 111 | htmlhelp_basename = "BlueInterfacedoc" 112 | 113 | 114 | # -- Options for Github output ------------------------------------------------ 115 | 116 | sphinx_to_github = True 117 | sphinx_to_github_verbose = True 118 | sphinx_to_github_encoding = "utf-8" 119 | 120 | 121 | # -- Options for LaTeX output ------------------------------------------------ 122 | 123 | latex_elements = { 124 | # The paper size ('letterpaper' or 'a4paper'). 125 | # 126 | # 'papersize': 'letterpaper', 127 | # The font size ('10pt', '11pt' or '12pt'). 128 | # 129 | # 'pointsize': '10pt', 130 | # Additional stuff for the LaTeX preamble. 131 | # 132 | # 'preamble': '', 133 | # Latex figure (float) alignment 134 | # 135 | # 'figure_align': 'htbp', 136 | } 137 | 138 | # Grouping the document tree into LaTeX files. List of tuples 139 | # (source start file, target name, title, 140 | # author, documentclass [howto, manual, or own class]). 141 | latex_documents = [ 142 | ( 143 | master_doc, 144 | "BlueInterface.tex", 145 | "Blue Interface Documentation", 146 | "Rachel Thomasson, Brent Yi", 147 | "manual", 148 | ), 149 | ] 150 | 151 | 152 | # -- Options for manual page output ------------------------------------------ 153 | 154 | # One entry per manual page. List of tuples 155 | # (source start file, name, description, authors, manual section). 156 | man_pages = [(master_doc, "blueinterface", "Blue Interface Documentation", [author], 1)] 157 | 158 | 159 | # -- Options for Texinfo output ---------------------------------------------- 160 | 161 | # Grouping the document tree into Texinfo files. List of tuples 162 | # (source start file, target name, title, author, 163 | # dir menu entry, description, category) 164 | texinfo_documents = [ 165 | ( 166 | master_doc, 167 | "BlueInterface", 168 | "Blue Interface Documentation", 169 | author, 170 | "BlueInterface", 171 | "One line description of project.", 172 | "Miscellaneous", 173 | ), 174 | ] 175 | 176 | 177 | # -- Extension configuration ------------------------------------------------- 178 | 179 | # -- Options for todo extension ---------------------------------------------- 180 | 181 | # If true, `todo` and `todoList` produce output, else they produce nothing. 182 | todo_include_todos = True 183 | -------------------------------------------------------------------------------- /docs/source/index.rst: -------------------------------------------------------------------------------- 1 | .. Blue Interface documentation master file, created by 2 | sphinx-quickstart on Thu Mar 8 00:09:36 2018. 3 | You can adapt this file completely to your liking, but it should at least 4 | contain the root `toctree` directive. 5 | 6 | Blue Interface API Documentation 7 | ========================================== 8 | 9 | Blue Interface is a platform-agnostic Python API for controlling Blue robotic arms over a network connection. 10 | 11 | It features: 12 | 13 | - No dependency on ROS (or any particular version of Ubuntu) 14 | - Easy connection to multiple robots 15 | - Support for both Python 2 and 3 16 | - Support for Mac, Windows, and Linux 17 | - Support for Jupyter Notebooks 18 | 19 | It's designed to be lightweight and easy-to-use! Sending a Blue "right" arm to its zero position, for example, is as simple as: 20 | 21 | .. code-block:: python 22 | 23 | from blue_interface import BlueInterface 24 | 25 | blue = BlueInterface(side="right", ip="127.0.0.1") 26 | blue.set_joint_positions([0] * 7) 27 | 28 | See Github_ for installation instructions and more usage examples. 29 | 30 | .. _Github: https://github.com/berkeleyopenarms/blue_interface 31 | 32 | .. .. toctree:: 33 | .. :maxdepth: 2 34 | .. :caption: Contents: 35 | 36 | ---- 37 | 38 | .. automodule:: blue_interface 39 | .. autoclass:: BlueInterface 40 | :members: 41 | :undoc-members: 42 | 43 | 44 | .. Indices and tables 45 | .. ================== 46 | .. 47 | .. * :ref:`genindex` 48 | .. * :ref:`modindex` 49 | .. * :ref:`search` 50 | -------------------------------------------------------------------------------- /examples/gripper_controller.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | # A basic example of using BlueInterface for gripper control. 4 | # It allows a user to open and close the gripper. 5 | 6 | from blue_interface import BlueInterface 7 | 8 | side = "right" 9 | ip = "127.0.0.1" 10 | blue = BlueInterface(side, ip) 11 | 12 | # After calibration, the gripper will be closed 13 | blue.calibrate_gripper() 14 | 15 | opened = False 16 | while True: 17 | input("Press enter to open/close the gripper. To exit, press .") 18 | 19 | if opened: 20 | print("Closing...") 21 | blue.command_gripper(-1.5, 20.0, wait=True) 22 | else: 23 | print("Opening...") 24 | blue.command_gripper(0.0, 10.0, wait=True) 25 | 26 | opened = not opened 27 | -------------------------------------------------------------------------------- /examples/inverse_kinematics.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | # A basic example of sending Blue a command in cartesian space. 4 | 5 | import time 6 | 7 | import numpy as np 8 | 9 | from blue_interface import BlueInterface 10 | 11 | side = "right" 12 | ip = "127.0.0.1" 13 | blue = BlueInterface(side, ip) 14 | 15 | # Compute IK solution 16 | target_position = [0.4, 0, 0] # x, y, z 17 | target_orientation = [0.6847088, -0.17378805, -0.69229771, -0.1472938] # x, y, z, w 18 | target_joint_positions = blue.inverse_kinematics(target_position, target_orientation) 19 | 20 | # Send command to robot 21 | blue.set_joint_positions(target_joint_positions, duration=5) 22 | 23 | # Wait for system to settle 24 | time.sleep(5) 25 | 26 | # Print results 27 | joint_positions = blue.get_joint_positions() 28 | pose = blue.get_cartesian_pose() 29 | 30 | 31 | def print_aligned(left, right): 32 | print("{:30} {}".format(left, np.round(right, 4))) 33 | 34 | 35 | print_aligned("Target joint positions: ", target_joint_positions) 36 | print_aligned("End joint positions: ", joint_positions) 37 | print_aligned("Target cartesian position:", target_position) 38 | print_aligned("End cartesian position:", pose["position"]) 39 | print_aligned("Target orientation:", target_orientation) 40 | print_aligned("End orientation:", pose["orientation"]) 41 | -------------------------------------------------------------------------------- /examples/print_status.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | # A basic example of reading information about Blue's current state. 4 | 5 | import time 6 | 7 | import numpy as np 8 | 9 | from blue_interface import BlueInterface 10 | 11 | side = "right" 12 | ip = "127.0.0.1" 13 | blue = BlueInterface(side, ip) 14 | 15 | 16 | def print_aligned(left, right): 17 | print("{:30} {}".format(left, np.round(right, 4))) 18 | 19 | 20 | while True: 21 | print_aligned("End Effector Position:", blue.get_cartesian_pose()["position"]) 22 | print_aligned("End Effector Orientation:", blue.get_cartesian_pose()["orientation"]) 23 | print_aligned("Joint Positions:", blue.get_joint_positions()) 24 | print_aligned("Joint Velocities:", blue.get_joint_velocities()) 25 | print_aligned("Joint Torques:", blue.get_joint_torques()) 26 | print_aligned("Gripper Position:", blue.get_gripper_position()) 27 | print_aligned("Gripper Effort:", blue.get_gripper_effort()) 28 | print("=" * 30) 29 | time.sleep(0.5) 30 | -------------------------------------------------------------------------------- /examples/zero_position.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | # A basic example of using BlueInterface for joint positions control. 4 | 5 | import numpy as np 6 | 7 | from blue_interface import BlueInterface 8 | 9 | side = "right" 10 | ip = "127.0.0.1" 11 | blue = BlueInterface(side, ip) 12 | 13 | blue.set_joint_positions(np.zeros(7), duration=3.0) 14 | 15 | while True: 16 | pass 17 | 18 | # When this script terminates (eg via Ctrl+C), the robot will automatically 19 | # stop all controllers and go back to gravity compensation mode 20 | -------------------------------------------------------------------------------- /mypy.ini: -------------------------------------------------------------------------------- 1 | [mypy] 2 | 3 | [mypy-setuptools] 4 | ignore_missing_imports = True 5 | 6 | [mypy-numpy] 7 | ignore_missing_imports = True 8 | 9 | [mypy-pydispatch] 10 | ignore_missing_imports = True 11 | 12 | [mypy-ws4py.client.threadedclient] 13 | ignore_missing_imports = True 14 | -------------------------------------------------------------------------------- /setup.py: -------------------------------------------------------------------------------- 1 | from setuptools import setup 2 | 3 | with open("README.md", "r") as fh: 4 | long_description = fh.read() 5 | 6 | setup( 7 | name="blue_interface", 8 | version="0.1", 9 | description="Python API for controlling the Blue robot arm", 10 | long_description=long_description, 11 | long_description_content_type="text/markdown", 12 | url="http://github.com/berkeleyopenarms/blue_interface", 13 | author="Rachel Thomasson, Brent Yi, Philipp Wu, Greg Balke", 14 | author_email="brentyi@berkeley.edu", 15 | license="BSD", 16 | packages=["blue_interface"], 17 | install_requires=["ws4py", "PyDispatcher", "numpy", "enum34", "typing"], 18 | zip_safe=False, 19 | classifiers=[ 20 | "Programming Language :: Python :: 2.7", 21 | "Programming Language :: Python :: 3.5", 22 | "Programming Language :: Python :: 3.6", 23 | "Programming Language :: Python :: 3.7", 24 | "Programming Language :: Python :: 3.8", 25 | "License :: OSI Approved :: MIT License", 26 | "Operating System :: OS Independent", 27 | ], 28 | ) 29 | -------------------------------------------------------------------------------- /tests/test_basic.py: -------------------------------------------------------------------------------- 1 | import pytest 2 | 3 | 4 | def test_no_rosbridge(): 5 | """Make sure that importing BlueInterface and instantiating it results in the correct 6 | error when no rosbridge server is running. 7 | 8 | (( assumes that no rosbridge server is running at localhost:444 )) 9 | """ 10 | 11 | from blue_interface import BlueInterface 12 | 13 | try: 14 | error = ConnectionRefusedError 15 | except NameError: 16 | # Python 2.7 17 | error = Exception 18 | 19 | with pytest.raises(error): 20 | BlueInterface(side="left", ip="127.0.0.1", port=444) 21 | --------------------------------------------------------------------------------