├── .gitignore ├── CHANGELOG.rst ├── CONTRIBUTING.md ├── LICENSE ├── README.rst ├── intera.sh ├── intera_examples ├── .gitignore ├── CHANGELOG.rst ├── CMakeLists.txt ├── LICENSE ├── README.rst ├── cfg │ └── SawyerJointSpringsExample.cfg ├── launch │ ├── gripper_joystick.launch │ ├── joint_position_joystick.launch │ ├── joint_trajectory_client.launch │ └── joint_trajectory_file_playback.launch ├── package.xml ├── scripts │ ├── camera_display.py │ ├── constrained_zeroG.py │ ├── fk_service_client.py │ ├── go_to_cartesian_pose.py │ ├── go_to_joint_angles.py │ ├── go_to_joint_angles_in_contact.py │ ├── gripper_cuff_control.py │ ├── gripper_joystick.py │ ├── gripper_keyboard.py │ ├── head_display_image.py │ ├── head_wobbler.py │ ├── ik_service_client.py │ ├── joint_position_file_playback.py │ ├── joint_position_joystick.py │ ├── joint_position_keyboard.py │ ├── joint_position_waypoints.py │ ├── joint_recorder.py │ ├── joint_torque_springs.py │ ├── joint_trajectory_client.py │ ├── joint_trajectory_file_playback.py │ ├── joint_velocity_wobbler.py │ ├── lights_blink.py │ ├── navigator_io.py │ ├── send_random_trajectory.py │ ├── send_traj_from_file.py │ ├── set_interaction_options.py │ └── stop_motion_trajectory.py ├── setup.py ├── share │ └── images │ │ ├── sawyer_and_baxter.png │ │ └── sawyer_sdk_research.png └── src │ ├── intera_examples │ ├── __init__.py │ └── recorder.py │ └── intera_external_devices │ ├── __init__.py │ ├── getch.py │ └── joystick.py ├── intera_interface ├── .gitignore ├── CHANGELOG.rst ├── CMakeLists.txt ├── LICENSE ├── README.rst ├── cfg │ ├── SawyerPositionFFJointTrajectoryActionServer.cfg │ ├── SawyerPositionJointTrajectoryActionServer.cfg │ └── SawyerVelocityJointTrajectoryActionServer.cfg ├── epydoc.config ├── package.xml ├── rosdoc.yaml ├── scripts │ ├── calibrate_arm.py │ ├── enable_robot.py │ ├── home_joints.py │ ├── io_config_editor.py │ ├── joint_trajectory_action_server.py │ └── send_urdf_fragment.py ├── setup.py └── src │ ├── intera_control │ ├── __init__.py │ └── pid.py │ ├── intera_dataflow │ ├── __init__.py │ ├── signals.py │ ├── wait_for.py │ └── weakrefset.py │ ├── intera_interface │ ├── __init__.py │ ├── camera.py │ ├── clicksmart_plate.py │ ├── cuff.py │ ├── digital_io.py │ ├── gripper.py │ ├── gripper_factory.py │ ├── head.py │ ├── head_display.py │ ├── joint_limits.py │ ├── lights.py │ ├── limb.py │ ├── navigator.py │ ├── robot_enable.py │ ├── robot_params.py │ └── settings.py │ ├── intera_io │ ├── __init__.py │ ├── io_command.py │ └── io_interface.py │ ├── intera_joint_trajectory_action │ ├── __init__.py │ ├── bezier.py │ ├── joint_trajectory_action.py │ └── minjerk.py │ └── intera_motion_interface │ ├── __init__.py │ ├── interaction_options.py │ ├── interaction_publisher.py │ ├── motion_controller_action_client.py │ ├── motion_trajectory.py │ ├── motion_waypoint.py │ ├── motion_waypoint_options.py │ ├── random_walk.py │ └── utility_functions.py └── intera_sdk ├── CHANGELOG.rst ├── CMakeLists.txt └── package.xml /.gitignore: -------------------------------------------------------------------------------- 1 | # Byte-compiled / optimized / DLL files 2 | __pycache__/ 3 | *.py[cod] 4 | *$py.class 5 | 6 | # C extensions 7 | *.so 8 | 9 | # Distribution / packaging 10 | .Python 11 | env/ 12 | build/ 13 | develop-eggs/ 14 | dist/ 15 | downloads/ 16 | eggs/ 17 | .eggs/ 18 | lib/ 19 | lib64/ 20 | parts/ 21 | sdist/ 22 | var/ 23 | *.egg-info/ 24 | .installed.cfg 25 | *.egg 26 | 27 | # PyInstaller 28 | # Usually these files are written by a python script from a template 29 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 30 | *.manifest 31 | *.spec 32 | 33 | # Installer logs 34 | pip-log.txt 35 | pip-delete-this-directory.txt 36 | 37 | # Unit test / coverage reports 38 | htmlcov/ 39 | .tox/ 40 | .coverage 41 | .coverage.* 42 | .cache 43 | nosetests.xml 44 | coverage.xml 45 | *,cover 46 | .hypothesis/ 47 | 48 | # Translations 49 | *.mo 50 | *.pot 51 | 52 | # Django stuff: 53 | *.log 54 | 55 | # Sphinx documentation 56 | docs/_build/ 57 | 58 | # PyBuilder 59 | target/ 60 | 61 | #Ipython Notebook 62 | .ipynb_checkpoints 63 | -------------------------------------------------------------------------------- /CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | 5.3.0 (2018-8-14) 2 | --------------------------------- 3 | - Interaction controls examples now return to postion mode when complete 4 | - Added support for randomized trajectories using motion controller 5 | - Fixed quaternion issue for set interaction options examples 6 | - Added stop motion trajectory examples 7 | - Modifications for RobotAssemblyState message rename 8 | - Added InteractionPublisher class for simpler interaction control 9 | 10 | 5.2.0 (2018-4-16) 11 | --------------------------------- 12 | - Added intera_motion_interface package for the motion controller 13 | - Added support for ClickSmart 14 | - Added constrained ZeroG example using interaction control 15 | - Various examples fixs and updates 16 | - Added camera configuration support 17 | 18 | 5.1.0 (2017-3-27) 19 | --------------------------------- 20 | - Moved to Apache 2.0 license 21 | - Added Contribution Guidelines 22 | - Fix error logging in IK and FK scripts 23 | - Adds default SDK head image to the repo for users to display 24 | - Ensures "reset" command is available only after an estop or robot error 25 | - Fixed issue with logic in gripper initialization and calibration 26 | 27 | 5.0.4 (2016-12-06) 28 | --------------------------------- 29 | - Intera SDK is compatible with the Sawyer SDK Robot 30 | - Initial release of intera_sdk intera_interface and intera_examples packages 31 | - See intera_interface API Docs: 32 | https://rethinkrobotics.github.io/intera_sdk_docs/5.0.4/intera_interface/html/index.html 33 | - Adds API classes for camera, cuff, gripper, head motion, head display, lights, limb, 34 | navigator, robot enabling, and robot parameters 35 | - Adds scripts for calibration, enabling the robot, homing the sawyer joints, urdf fragment updating, 36 | and an entry point for the intera joint trajectory action server 37 | - Adds intera_examples package with 19 code examples for interfacing with the robot 38 | - Adds intera_sdk metapackage for intera_examples and intera_interface 39 | -------------------------------------------------------------------------------- /CONTRIBUTING.md: -------------------------------------------------------------------------------- 1 | # How to Contribute 2 | 3 | We welcome contributions to this repo and encourage you to fork the project. 4 | Thank you for your interest and time spent contributing! 5 | 6 | ## Issues and Pull Requests 7 | 8 | If you are submitting an issue or a pull request, please prefix the title of the issue 9 | or Pull Requests with the package name. 10 | 11 | For Pull Requests, please target the `development` branch for any contributions. 12 | To contribute, please check out the `development` branch, and then create your feature 13 | branch from there: 14 | ``` 15 | $ git checkout development # start with the development 16 | $ git pull origin development # pull remote repo changes 17 | $ git checkout your-feature-branch # create your feature branch 18 | ``` 19 | Then when you submit a Pull Request, please select the `development` branch to request 20 | to merge your commits. 21 | 22 | If you are interested in understanding this development style a bit further, 23 | we follow the [Git Flow](http://nvie.com/posts/a-successful-git-branching-model/) 24 | model of branching, meaning that the `master` branch should be "stable". The quick 25 | summary is that only release branches and bug-hotfixes are committed in the `master` 26 | branch. The `release-*.*`branches are used for testing and bugfixes only. 27 | 28 | ## Git Commit Messages 29 | 30 | - Use the present tense ("Add feature" not "Added feature") 31 | - Use the imperative mood ("Remove dependency..." not "Removes dependency...") 32 | - Limit the first line to 50 characters or less, and subsequent lines to 72 or less 33 | - Reference issues and pull requests liberally with a hash and Issue or PR number 34 | 35 | ## Style 36 | 37 | We follow the [C++ ROS style guidelines](http://ros.org/wiki/CppStyleGuide) and 38 | [Python ROS style guidelines](http://wiki.ros.org/PyStyleGuide) as closely as possible. 39 | Please ensure that any new contributions are compatible with C++11, Python2.7, and Python3.x. 40 | -------------------------------------------------------------------------------- /README.rst: -------------------------------------------------------------------------------- 1 | Intera SDK 2 | ============== 3 | 4 | The Intera SDK provides a platform for development of custom applications for Intera Robots. 5 | 6 | This repository contains metapackages and files for installation/use of the Intera SDK. 7 | Additionally, this repositories contains the Python interface classes and examples for 8 | action servers and control of the Intera Robot from Rethink Robotics. 9 | 10 | Supported ROS Versions 11 | ------------ 12 | - Indigo 13 | - Kinetic 14 | - Melodic 15 | - Noetic 16 | 17 | Installation 18 | ------------ 19 | | Please follow the Getting Started wiki page for instructions on installation of the Intera SDK: 20 | | http://sdk.rethinkrobotics.com/intera/Workstation_Setup 21 | 22 | Code & Tickets 23 | -------------- 24 | 25 | +-----------------+----------------------------------------------------------------------------+ 26 | | Documentation | http://sdk.rethinkrobotics.com/intera/ | 27 | +-----------------+----------------------------------------------------------------------------+ 28 | | Python API Docs | http://rethinkrobotics.github.io/intera_sdk_docs | 29 | +-----------------+----------------------------------------------------------------------------+ 30 | | Issues | https://github.com/RethinkRobotics/intera_sdk/issues | 31 | +-----------------+----------------------------------------------------------------------------+ 32 | | Contributions | https://github.com/RethinkRobotics/intera_sdk/blob/master/CONTRIBUTING.md | 33 | +-----------------+----------------------------------------------------------------------------+ 34 | 35 | Intera Repository Overview 36 | -------------------------- 37 | 38 | :: 39 | 40 | . 41 | | 42 | +-- intera_sdk/ intera_sdk metapackage containing all intera sdk packages 43 | | 44 | +-- intera_interface python API for communicating with Intera-enabled robots 45 | | +-- cfg/ 46 | | +-- scripts/ 47 | | +-- src/ 48 | | 49 | +-- intera_examples examples using the Python API for Intera-enabled robots 50 | | +-- cfg/ 51 | | +-- launch/ 52 | | +-- share/images 53 | | +-- scripts/ 54 | | +-- src/ 55 | | 56 | +-- intera.sh convenient environment initialization script 57 | 58 | 59 | Other Intera Repositories 60 | ------------------------- 61 | +------------------+-----------------------------------------------------+ 62 | | intera_common | https://github.com/RethinkRobotics/intera_common | 63 | +------------------+-----------------------------------------------------+ 64 | 65 | Latest Release Information 66 | -------------------------- 67 | 68 | http://sdk.rethinkrobotics.com/intera/Release-Changes 69 | -------------------------------------------------------------------------------- /intera.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | # Copyright (c) 2013-2016, Rethink Robotics 3 | # All rights reserved. 4 | 5 | # This file is to be used in the *root* of your Catkin workspace. 6 | 7 | # This is a convenient script which will set up your ROS environment and 8 | # should be executed with every new instance of a shell in which you plan on 9 | # working with Intera. 10 | 11 | # Clear any previously set your_ip/your_hostname 12 | unset your_ip 13 | unset your_hostname 14 | #-----------------------------------------------------------------------------# 15 | # USER CONFIGURABLE ROS ENVIRONMENT VARIABLES # 16 | #-----------------------------------------------------------------------------# 17 | # Note: If ROS_MASTER_URI, ROS_IP, or ROS_HOSTNAME environment variables were 18 | # previously set (typically in your .bashrc or .bash_profile), those settings 19 | # will be overwritten by any variables set here. 20 | 21 | # Specify Robot's hostname 22 | robot_hostname="robot_hostname.local" 23 | 24 | # Set *Either* your computers ip address or hostname. Please note if using 25 | # your_hostname that this must be resolvable to the Robot. 26 | your_ip="192.168.XXX.XXX" 27 | #your_hostname="my_computer.local" 28 | 29 | # Specify ROS distribution (e.g. kinetic, indigo, hydro, etc.) 30 | ros_version="kinetic" 31 | #-----------------------------------------------------------------------------# 32 | 33 | tf=$(mktemp) 34 | trap "rm -f -- '${tf}'" EXIT 35 | 36 | # If this file specifies an ip address or hostname - unset any previously set 37 | # ROS_IP and/or ROS_HOSTNAME. 38 | # If this file does not specify an ip address or hostname - use the 39 | # previously specified ROS_IP/ROS_HOSTNAME environment variables. 40 | if [ -n "${your_ip}" ] || [ -n "${your_hostname}" ]; then 41 | unset ROS_IP && unset ROS_HOSTNAME 42 | else 43 | your_ip="${ROS_IP}" && your_hostname="${ROS_HOSTNAME}" 44 | fi 45 | 46 | # If argument provided, set robot_hostname to argument 47 | # If argument is sim or local, set robot_hostname to localhost 48 | if [ -n "${1}" ]; then 49 | if [[ "${1}" == "sim" ]] || [[ "${1}" == "local" ]]; then 50 | robot_hostname="localhost" 51 | if [[ -z ${your_ip} || "${your_ip}" == "192.168.XXX.XXX" ]] && \ 52 | [[ -z ${your_hostname} || "${your_hostname}" == "my_computer.local" ]]; then 53 | your_hostname="localhost" 54 | your_ip="" 55 | fi 56 | else 57 | robot_hostname="${1}" 58 | fi 59 | fi 60 | 61 | topdir=$(basename $(readlink -f $(dirname ${BASH_SOURCE[0]}))) 62 | 63 | cat <<-EOF > ${tf} 64 | [ -s "\${HOME}"/.bashrc ] && source "\${HOME}"/.bashrc 65 | [ -s "\${HOME}"/.bash_profile ] && source "\${HOME}"/.bash_profile 66 | 67 | # verify this script is moved out of intera_sdk folder 68 | if [[ -e "${topdir}/intera_sdk/package.xml" ]]; then 69 | echo -ne "EXITING - This script must be moved from the intera_sdk folder \ 70 | to the root of your catkin workspace.\n" 71 | exit 1 72 | fi 73 | 74 | # verify ros_version lowercase 75 | ros_version="$(tr [A-Z] [a-z] <<< "${ros_version}")" 76 | 77 | # check for ros installation 78 | if [ ! -d "/opt/ros" ] || [ ! "$(ls -A /opt/ros)" ]; then 79 | echo "EXITING - No ROS installation found in /opt/ros." 80 | echo "Is ROS installed?" 81 | exit 1 82 | fi 83 | 84 | # if set, verify user has modified the robot_hostname 85 | if [ -n ${robot_hostname} ] && \ 86 | [[ "${robot_hostname}" == "robot_hostname.local" ]]; then 87 | echo -ne "EXITING - Please edit this file, modifying the \ 88 | 'robot_hostname' variable to reflect your Robot's current hostname.\n" 89 | exit 1 90 | fi 91 | 92 | # if set, verify user has modified their ip address (your_ip) 93 | if [ -n ${your_ip} ] && [[ "${your_ip}" == "192.168.XXX.XXX" ]]; then 94 | echo -ne "EXITING - Please edit this file, modifying the 'your_ip' \ 95 | variable to reflect your current IP address.\n" 96 | exit 1 97 | fi 98 | 99 | # if set, verify user has modified their computer hostname (your_hostname) 100 | if [ -n ${your_hostname} ] && \ 101 | [[ "${your_hostname}" == "my_computer.local" ]]; then 102 | echo -ne "EXITING - Please edit this file, modifying the \ 103 | 'your_hostname' variable to reflect your current PC hostname.\n" 104 | exit 1 105 | fi 106 | 107 | # verify user does not have both their ip *and* hostname set 108 | if [ -n "${your_ip}" ] && [ -n "${your_hostname}" ]; then 109 | echo -ne "EXITING - Please edit this file, modifying to specify \ 110 | *EITHER* your_ip or your_hostname.\n" 111 | exit 1 112 | fi 113 | 114 | # verify that one of your_ip, your_hostname, ROS_IP, or ROS_HOSTNAME is set 115 | if [ -z "${your_ip}" ] && [ -z "${your_hostname}" ]; then 116 | echo -ne "EXITING - Please edit this file, modifying to specify \ 117 | your_ip or your_hostname.\n" 118 | exit 1 119 | fi 120 | 121 | # verify specified ros version is installed 122 | ros_setup="/opt/ros/\${ros_version}" 123 | if [ ! -d "\${ros_setup}" ]; then 124 | echo -ne "EXITING - Failed to find ROS \${ros_version} installation \ 125 | in \${ros_setup}.\n" 126 | exit 1 127 | fi 128 | 129 | # verify the ros setup.sh file exists 130 | if [ ! -s "\${ros_setup}"/setup.sh ]; then 131 | echo -ne "EXITING - Failed to find the ROS environment script: \ 132 | "\${ros_setup}"/setup.sh.\n" 133 | exit 1 134 | fi 135 | 136 | # verify the user is running this script in the root of the catkin 137 | # workspace and that the workspace has been compiled. 138 | if [ ! -s "devel/setup.bash" ]; then 139 | if [ ! -s "install/setup.sh" ]; then 140 | echo -ne "EXITING - \n1) Please verify that this script is being run \ 141 | in the root of your catkin workspace.\n2) Please verify that your workspace \ 142 | has been built (source /opt/ros/\${ros_version}/setup.sh; catkin_make).\n\ 143 | 3) Run this script again upon completion of your workspace build.\n" 144 | exit 1 145 | fi 146 | fi 147 | 148 | [ -n "${your_ip}" ] && export ROS_IP="${your_ip}" 149 | [ -n "${your_hostname}" ] && export ROS_HOSTNAME="${your_hostname}" 150 | [ -n "${robot_hostname}" ] && \ 151 | export ROS_MASTER_URI="http://${robot_hostname}:11311" 152 | 153 | # source the catkin setup bash script 154 | source devel/setup.bash 155 | 156 | # setup the bash prompt 157 | export __ROS_PROMPT=\${__ROS_PROMPT:-0} 158 | [ \${__ROS_PROMPT} -eq 0 -a -n "\${PROMPT_COMMAND}" ] && \ 159 | export __ORIG_PROMPT_COMMAND=\${PROMPT_COMMAND} 160 | 161 | __ros_prompt () { 162 | if [ -n "\${__ORIG_PROMPT_COMMAND}" ]; then 163 | eval \${__ORIG_PROMPT_COMMAND} 164 | fi 165 | if ! echo \${PS1} | grep '\[intera' &>/dev/null; then 166 | export PS1="\[\033[00;33m\][intera - \ 167 | \${ROS_MASTER_URI}]\[\033[00m\] \${PS1}" 168 | fi 169 | } 170 | 171 | if [ "\${TERM}" != "dumb" ]; then 172 | export PROMPT_COMMAND=__ros_prompt 173 | __ROS_PROMPT=1 174 | elif ! echo \${PS1} | grep '\[intera' &>/dev/null; then 175 | export PS1="[intera - \${ROS_MASTER_URI}] \${PS1}" 176 | fi 177 | 178 | EOF 179 | 180 | ${SHELL} --rcfile ${tf} 181 | 182 | rm -f -- "${tf}" 183 | trap - EXIT 184 | 185 | # vim: noet 186 | -------------------------------------------------------------------------------- /intera_examples/.gitignore: -------------------------------------------------------------------------------- 1 | *.py[cod] 2 | 3 | # C extensions 4 | *.so 5 | 6 | # Packages 7 | *.egg 8 | *.egg-info 9 | dist 10 | build 11 | eggs 12 | parts 13 | bin 14 | var 15 | sdist 16 | develop-eggs 17 | .installed.cfg 18 | lib 19 | lib64 20 | 21 | # Installer logs 22 | pip-log.txt 23 | 24 | # Unit test / coverage reports 25 | .coverage 26 | .tox 27 | nosetests.xml 28 | 29 | # Translations 30 | *.mo 31 | 32 | # Mr Developer 33 | .mr.developer.cfg 34 | .project 35 | .cproject 36 | cmake_install.cmake 37 | .pydevproject 38 | *.swp 39 | CATKIN_IGNORE 40 | catkin 41 | catkin_generated 42 | devel 43 | 44 | cpp 45 | docs 46 | msg_gen 47 | srv_gen 48 | *.smoketest 49 | *.cfgc 50 | *_msgs/src/ 51 | */src/**/cfg 52 | */*/src/**/* 53 | -------------------------------------------------------------------------------- /intera_examples/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | 5.0.3 (2016-12-05) 2 | --------------------------------- 3 | - Initial release of interai_examples, borrowing heavily from baxter_examples 4 | but robot-agnostic to any robot running Intera 5 | 6 | -------------------------------------------------------------------------------- /intera_examples/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(intera_examples) 3 | 4 | find_package(catkin 5 | REQUIRED 6 | COMPONENTS 7 | rospy 8 | actionlib 9 | sensor_msgs 10 | control_msgs 11 | trajectory_msgs 12 | cv_bridge 13 | dynamic_reconfigure 14 | intera_core_msgs 15 | intera_motion_msgs 16 | intera_interface 17 | ) 18 | 19 | catkin_python_setup() 20 | 21 | generate_dynamic_reconfigure_options( 22 | cfg/SawyerJointSpringsExample.cfg 23 | ) 24 | 25 | catkin_package( 26 | CATKIN_DEPENDS 27 | rospy 28 | actionlib 29 | sensor_msgs 30 | control_msgs 31 | trajectory_msgs 32 | cv_bridge 33 | dynamic_reconfigure 34 | intera_core_msgs 35 | intera_motion_msgs 36 | intera_interface 37 | ) 38 | 39 | catkin_install_python(PROGRAMS 40 | scripts/ik_service_client.py 41 | scripts/go_to_joint_angles.py 42 | scripts/joint_position_waypoints.py 43 | scripts/joint_torque_springs.py 44 | scripts/gripper_cuff_control.py 45 | scripts/head_wobbler.py 46 | scripts/joint_recorder.py 47 | scripts/fk_service_client.py 48 | scripts/joint_velocity_wobbler.py 49 | scripts/navigator_io.py 50 | scripts/joint_position_joystick.py 51 | scripts/joint_position_keyboard.py 52 | scripts/gripper_keyboard.py 53 | scripts/stop_motion_trajectory.py 54 | scripts/go_to_joint_angles_in_contact.py 55 | scripts/joint_position_file_playback.py 56 | scripts/go_to_cartesian_pose.py 57 | scripts/head_display_image.py 58 | scripts/joint_trajectory_client.py 59 | scripts/joint_trajectory_file_playback.py 60 | scripts/set_interaction_options.py 61 | scripts/constrained_zeroG.py 62 | scripts/gripper_joystick.py 63 | scripts/lights_blink.py 64 | scripts/camera_display.py 65 | scripts/send_random_trajectory.py 66 | scripts/send_traj_from_file.py 67 | 68 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) 69 | 70 | install( 71 | DIRECTORY scripts/ 72 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 73 | USE_SOURCE_PERMISSIONS 74 | ) 75 | 76 | install( 77 | DIRECTORY launch/ 78 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch 79 | USE_SOURCE_PERMISSIONS 80 | ) 81 | 82 | install( 83 | DIRECTORY share/ 84 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/share 85 | USE_SOURCE_PERMISSIONS 86 | ) 87 | -------------------------------------------------------------------------------- /intera_examples/README.rst: -------------------------------------------------------------------------------- 1 | intera_examples 2 | =============== 3 | 4 | Examples for utilizing the Intera SDK Interface to communicate with robots from Rethink Robotics 5 | 6 | Code & Tickets 7 | -------------- 8 | 9 | +-----------------+----------------------------------------------------------------+ 10 | | Documentation | http://sdk.rethinkrobotics.com/intera/ | 11 | +-----------------+----------------------------------------------------------------+ 12 | | Issues | https://github.com/RethinkRobotics/intera_sdk/issues | 13 | +-----------------+----------------------------------------------------------------+ 14 | | Contributions | http://sdk.rethinkrobotics.com/intera/Contributions | 15 | +-----------------+----------------------------------------------------------------+ 16 | 17 | intera_examples Repository Overview 18 | ----------------------------------- 19 | 20 | :: 21 | 22 | . 23 | | 24 | +-- scripts/ example program executables 25 | | +-- camera_display.py 26 | | +-- fk_service_client.py 27 | | +-- gripper_cuff_control.py 28 | | +-- gripper_joystick.py 29 | | +-- gripper_keyboard.py 30 | | +-- head_display_image.py 31 | | +-- head_wobbler.py 32 | | +-- ik_service_client.py 33 | | +-- joint_position_file_playback.py 34 | | +-- joint_position_joystick.py 35 | | +-- joint_position_keyboard.py 36 | | +-- joint_position_waypoints.py 37 | | +-- joint_recorder.py 38 | | +-- joint_torque_springs.py 39 | | +-- joint_trajectory_client.py 40 | | +-- joint_trajectory_file_playback.py 41 | | +-- joint_velocity_puppet.py 42 | | +-- joint_velocity_wobbler.py 43 | | +-- lights_blink.py 44 | | +-- navigator_io.py 45 | | 46 | +-- launch/ example program launch scripts 47 | | +-- gripper_joystick.launch 48 | | +-- joint_position_joystick.launch 49 | | +-- joint_trajectory_client.launch 50 | | +-- joint_trajectory_file_playback.launch 51 | | 52 | +-- src/ intera_examples api 53 | | +-- intera_examples/ example classes 54 | | +-- intera_external_devices/ external device classes 55 | | 56 | +-- share/ shared example program resources 57 | | 58 | +-- cfg/ dynamic reconfigure example configs 59 | 60 | 61 | Other Intera SDK Repositories 62 | ------------------------- 63 | 64 | +------------------+-----------------------------------------------------+ 65 | | intera_common | https://github.com/RethinkRobotics/intera_common | 66 | +------------------+-----------------------------------------------------+ 67 | 68 | Latest Release Information 69 | -------------------------- 70 | 71 | http://sdk.rethinkrobotics.com/intera/Release-Changes 72 | -------------------------------------------------------------------------------- /intera_examples/cfg/SawyerJointSpringsExample.cfg: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | # Copyright (c) 2013-2018, Rethink Robotics Inc. 3 | # 4 | # Licensed under the Apache License, Version 2.0 (the "License"); 5 | # you may not use this file except in compliance with the License. 6 | # You may obtain a copy of the License at 7 | # 8 | # http://www.apache.org/licenses/LICENSE-2.0 9 | # 10 | # Unless required by applicable law or agreed to in writing, software 11 | # distributed under the License is distributed on an "AS IS" BASIS, 12 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | # See the License for the specific language governing permissions and 14 | # limitations under the License. 15 | 16 | from dynamic_reconfigure.parameter_generator_catkin import ( 17 | ParameterGenerator, 18 | double_t, 19 | ) 20 | 21 | gen = ParameterGenerator() 22 | 23 | joints = ('j0', 'j1', 'j2', 'j3', 'j4', 'j5', 'j6') 24 | 25 | msg = (" - Joint spring stiffness (k). Hooke's Law.", 26 | " - Joint damping coefficient (c).") 27 | min = 0.0 28 | default_spring = (10.0, 15.0, 5.0, 5.0, 3.0, 2.0, 1.5) 29 | default_damping = (0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1) 30 | max_spring = (30.0, 30.0, 15.0, 15.0, 9.0, 4.0, 4.0) 31 | max_damping = (10.0, 7.5, 7.5, 5.0, 1.5, 1.5, 1.0) 32 | 33 | for idx, joint in enumerate(joints): 34 | gen.add( 35 | joint + "_spring_stiffness", double_t, 0, joint + msg[0], 36 | default_spring[idx], min, max_spring[idx] 37 | ) 38 | gen.add( 39 | joint + "_damping_coefficient", double_t, 0, joint + msg[1], 40 | default_damping[idx], min, max_damping[idx] 41 | ) 42 | 43 | exit(gen.generate('joint_torque', '', 'SawyerJointSpringsExample')) 44 | -------------------------------------------------------------------------------- /intera_examples/launch/gripper_joystick.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 12 | 13 | 14 | 15 | 16 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /intera_examples/launch/joint_position_joystick.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 12 | 13 | 14 | 15 | 16 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /intera_examples/launch/joint_trajectory_client.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 10 | 11 | 12 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /intera_examples/launch/joint_trajectory_file_playback.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 13 | 14 | 15 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /intera_examples/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | intera_examples 4 | 5.3.0 5 | 6 | Example programs for Intera SDK usage. 7 | 8 | 9 | 10 | Rethink Robotics Inc. 11 | 12 | Apache 2.0 13 | http://sdk.rethinkrobotics.com/intera/ 14 | 15 | https://github.com/RethinkRobotics/intera_sdk 16 | 17 | 18 | https://github.com/RethinkRobotics/intera_sdk/issues 19 | 20 | Rethink Robotics Inc. 21 | 22 | catkin 23 | 24 | python-setuptools 25 | python3-setuptools 26 | rospy 27 | actionlib 28 | sensor_msgs 29 | control_msgs 30 | trajectory_msgs 31 | cv_bridge 32 | dynamic_reconfigure 33 | intera_core_msgs 34 | intera_motion_msgs 35 | intera_interface 36 | 37 | rospy 38 | actionlib 39 | sensor_msgs 40 | control_msgs 41 | trajectory_msgs 42 | cv_bridge 43 | dynamic_reconfigure 44 | intera_core_msgs 45 | intera_motion_msgs 46 | intera_interface 47 | joystick_drivers 48 | 49 | 50 | -------------------------------------------------------------------------------- /intera_examples/scripts/camera_display.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | # Copyright (c) 2013-2018, Rethink Robotics Inc. 3 | # 4 | # Licensed under the Apache License, Version 2.0 (the "License"); 5 | # you may not use this file except in compliance with the License. 6 | # You may obtain a copy of the License at 7 | # 8 | # http://www.apache.org/licenses/LICENSE-2.0 9 | # 10 | # Unless required by applicable law or agreed to in writing, software 11 | # distributed under the License is distributed on an "AS IS" BASIS, 12 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | # See the License for the specific language governing permissions and 14 | # limitations under the License. 15 | 16 | import argparse 17 | import numpy as np 18 | 19 | import cv2 20 | from cv_bridge import CvBridge, CvBridgeError 21 | 22 | import rospy 23 | import intera_interface 24 | 25 | def show_image_callback(img_data, xxx_todo_changeme): 26 | """The callback function to show image by using CvBridge and cv 27 | """ 28 | (edge_detection, window_name) = xxx_todo_changeme 29 | bridge = CvBridge() 30 | try: 31 | cv_image = bridge.imgmsg_to_cv2(img_data, "bgr8") 32 | except CvBridgeError as err: 33 | rospy.logerr(err) 34 | return 35 | if edge_detection == True: 36 | gray = cv2.cvtColor(cv_image, cv2.COLOR_BGR2GRAY) 37 | blurred = cv2.GaussianBlur(gray, (3, 3), 0) 38 | # customize the second and the third argument, minVal and maxVal 39 | # in function cv2.Canny if needed 40 | get_edge = cv2.Canny(blurred, 10, 100) 41 | cv_image = np.hstack([get_edge]) 42 | edge_str = "(Edge Detection)" if edge_detection else '' 43 | cv_win_name = ' '.join([window_name, edge_str]) 44 | cv2.namedWindow(cv_win_name, 0) 45 | # refresh the image on the screen 46 | cv2.imshow(cv_win_name, cv_image) 47 | cv2.waitKey(3) 48 | 49 | def main(): 50 | """Camera Display Example 51 | 52 | Cognex Hand Camera Ranges 53 | - exposure: [0.01-100] 54 | - gain: [0-255] 55 | Head Camera Ranges: 56 | - exposure: [0-100], -1 for auto-exposure 57 | - gain: [0-79], -1 for auto-gain 58 | """ 59 | rp = intera_interface.RobotParams() 60 | valid_cameras = rp.get_camera_names() 61 | if not valid_cameras: 62 | rp.log_message(("Cannot detect any camera_config" 63 | " parameters on this robot. Exiting."), "ERROR") 64 | return 65 | arg_fmt = argparse.RawDescriptionHelpFormatter 66 | parser = argparse.ArgumentParser(formatter_class=arg_fmt, 67 | description=main.__doc__) 68 | parser.add_argument( 69 | '-c', '--camera', type=str, default="head_camera", 70 | choices=valid_cameras, help='Setup Camera Name for Camera Display') 71 | parser.add_argument( 72 | '-r', '--raw', action='store_true', 73 | help='Specify use of the raw image (unrectified) topic') 74 | parser.add_argument( 75 | '-e', '--edge', action='store_true', 76 | help='Streaming the Canny edge detection image') 77 | parser.add_argument( 78 | '-g', '--gain', type=int, 79 | help='Set gain for camera (-1 = auto)') 80 | parser.add_argument( 81 | '-x', '--exposure', type=float, 82 | help='Set exposure for camera (-1 = auto)') 83 | args = parser.parse_args(rospy.myargv()[1:]) 84 | 85 | print("Initializing node... ") 86 | rospy.init_node('camera_display', anonymous=True) 87 | cameras = intera_interface.Cameras() 88 | if not cameras.verify_camera_exists(args.camera): 89 | rospy.logerr("Could not detect the specified camera, exiting the example.") 90 | return 91 | rospy.loginfo("Opening camera '{0}'...".format(args.camera)) 92 | cameras.start_streaming(args.camera) 93 | rectify_image = not args.raw 94 | use_canny_edge = args.edge 95 | cameras.set_callback(args.camera, show_image_callback, 96 | rectify_image=rectify_image, callback_args=(use_canny_edge, args.camera)) 97 | 98 | # optionally set gain and exposure parameters 99 | if args.gain is not None: 100 | if cameras.set_gain(args.camera, args.gain): 101 | rospy.loginfo("Gain set to: {0}".format(cameras.get_gain(args.camera))) 102 | 103 | if args.exposure is not None: 104 | if cameras.set_exposure(args.camera, args.exposure): 105 | rospy.loginfo("Exposure set to: {0}".format(cameras.get_exposure(args.camera))) 106 | 107 | def clean_shutdown(): 108 | print("Shutting down camera_display node.") 109 | cv2.destroyAllWindows() 110 | 111 | rospy.on_shutdown(clean_shutdown) 112 | rospy.loginfo("Camera_display node running. Ctrl-c to quit") 113 | rospy.spin() 114 | 115 | 116 | if __name__ == '__main__': 117 | main() 118 | -------------------------------------------------------------------------------- /intera_examples/scripts/fk_service_client.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | # Copyright (c) 2013-2018, Rethink Robotics Inc. 3 | # 4 | # Licensed under the Apache License, Version 2.0 (the "License"); 5 | # you may not use this file except in compliance with the License. 6 | # You may obtain a copy of the License at 7 | # 8 | # http://www.apache.org/licenses/LICENSE-2.0 9 | # 10 | # Unless required by applicable law or agreed to in writing, software 11 | # distributed under the License is distributed on an "AS IS" BASIS, 12 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | # See the License for the specific language governing permissions and 14 | # limitations under the License. 15 | 16 | """ 17 | Intera RSDK Forward Kinematics Example 18 | """ 19 | import rospy 20 | from geometry_msgs.msg import ( 21 | PoseStamped, 22 | Pose, 23 | Point, 24 | Quaternion, 25 | ) 26 | from std_msgs.msg import Header 27 | from sensor_msgs.msg import JointState 28 | 29 | from intera_core_msgs.srv import ( 30 | SolvePositionFK, 31 | SolvePositionFKRequest, 32 | ) 33 | 34 | def fk_service_client(limb = "right"): 35 | ns = "ExternalTools/" + limb + "/PositionKinematicsNode/FKService" 36 | fksvc = rospy.ServiceProxy(ns, SolvePositionFK) 37 | fkreq = SolvePositionFKRequest() 38 | joints = JointState() 39 | joints.name = ['right_j0', 'right_j1', 'right_j2', 'right_j3', 40 | 'right_j4', 'right_j5', 'right_j6'] 41 | joints.position = [0.763331, 0.415979, -1.728629, 1.482985, 42 | -1.135621, -1.674347, -0.496337] 43 | # Add desired pose for forward kinematics 44 | fkreq.configuration.append(joints) 45 | # Request forward kinematics from base to "right_hand" link 46 | fkreq.tip_names.append('right_hand') 47 | 48 | try: 49 | rospy.wait_for_service(ns, 5.0) 50 | resp = fksvc(fkreq) 51 | except (rospy.ServiceException, rospy.ROSException) as e: 52 | rospy.logerr("Service call failed: %s" % (e,)) 53 | return False 54 | 55 | # Check if result valid 56 | if (resp.isValid[0]): 57 | rospy.loginfo("SUCCESS - Valid Cartesian Solution Found") 58 | rospy.loginfo("\nFK Cartesian Solution:\n") 59 | rospy.loginfo("------------------") 60 | rospy.loginfo("Response Message:\n%s", resp) 61 | else: 62 | rospy.logerr("INVALID JOINTS - No Cartesian Solution Found.") 63 | return False 64 | 65 | return True 66 | 67 | 68 | def main(): 69 | """RSDK Forward Kinematics Example 70 | 71 | A simple example of using the Rethink Forward Kinematics 72 | Service which returns the Cartesian Pose based on the input joint angles. 73 | 74 | Run this example, the example will use the default limb 75 | and call the Service with a sample Joint Position, 76 | pre-defined in the example code, printing the 77 | response of whether a valid Cartesian solution was found, 78 | and if so, the corresponding Cartesian pose. 79 | """ 80 | rospy.init_node("rsdk_fk_service_client") 81 | 82 | if fk_service_client(): 83 | rospy.loginfo("Simple FK call passed!") 84 | else: 85 | rospy.logerr("Simple FK call FAILED") 86 | 87 | 88 | if __name__ == '__main__': 89 | main() 90 | -------------------------------------------------------------------------------- /intera_examples/scripts/go_to_joint_angles.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | # Copyright (c) 2016-2018, Rethink Robotics Inc. 3 | # 4 | # Licensed under the Apache License, Version 2.0 (the "License"); 5 | # you may not use this file except in compliance with the License. 6 | # You may obtain a copy of the License at 7 | # 8 | # http://www.apache.org/licenses/LICENSE-2.0 9 | # 10 | # Unless required by applicable law or agreed to in writing, software 11 | # distributed under the License is distributed on an "AS IS" BASIS, 12 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | # See the License for the specific language governing permissions and 14 | # limitations under the License. 15 | 16 | import rospy 17 | import argparse 18 | from intera_motion_interface import ( 19 | MotionTrajectory, 20 | MotionWaypoint, 21 | MotionWaypointOptions 22 | ) 23 | from intera_interface import Limb 24 | 25 | def main(): 26 | """ 27 | Move the robot arm to the specified configuration. 28 | Call using: 29 | $ rosrun intera_examples go_to_joint_angles.py [arguments: see below] 30 | 31 | -q 0.0 0.0 0.0 0.0 0.0 0.0 0.0 32 | --> Go to joint pose: 0.0 0.0 0.0 0.0 0.0 0.0 0.0 using default settings 33 | 34 | -q 0.1 -0.2 0.15 -0.05 -0.08 0.14 -0.04 -s 0.1 35 | --> Go to pose [...] with a speed ratio of 0.1 36 | 37 | -q -0.2 0.1 0.1 0.2 -0.3 0.2 0.4 -s 0.9 -a 0.1 38 | --> Go to pose [...] with a speed ratio of 0.9 and an accel ratio of 0.1 39 | """ 40 | arg_fmt = argparse.RawDescriptionHelpFormatter 41 | parser = argparse.ArgumentParser(formatter_class=arg_fmt, 42 | description=main.__doc__) 43 | parser.add_argument( 44 | "-q", "--joint_angles", type=float, 45 | nargs='+', default=[0.0, -0.9, 0.0, 1.8, 0.0, -0.9, 0.0], 46 | help="A list of joint angles, one for each of the 7 joints, J0...J6") 47 | parser.add_argument( 48 | "-s", "--speed_ratio", type=float, default=0.5, 49 | help="A value between 0.001 (slow) and 1.0 (maximum joint velocity)") 50 | parser.add_argument( 51 | "-a", "--accel_ratio", type=float, default=0.5, 52 | help="A value between 0.001 (slow) and 1.0 (maximum joint accel)") 53 | parser.add_argument( 54 | "--timeout", type=float, default=None, 55 | help="Max time in seconds to complete motion goal before returning. None is interpreted as an infinite timeout.") 56 | args = parser.parse_args(rospy.myargv()[1:]) 57 | 58 | try: 59 | rospy.init_node('go_to_joint_angles_py') 60 | limb = Limb() 61 | traj = MotionTrajectory(limb = limb) 62 | 63 | wpt_opts = MotionWaypointOptions(max_joint_speed_ratio=args.speed_ratio, 64 | max_joint_accel=args.accel_ratio) 65 | waypoint = MotionWaypoint(options = wpt_opts.to_msg(), limb = limb) 66 | 67 | joint_angles = limb.joint_ordered_angles() 68 | 69 | waypoint.set_joint_angles(joint_angles = joint_angles) 70 | traj.append_waypoint(waypoint.to_msg()) 71 | 72 | if len(args.joint_angles) != len(joint_angles): 73 | rospy.logerr('The number of joint_angles must be %d', len(joint_angles)) 74 | return None 75 | 76 | waypoint.set_joint_angles(joint_angles = args.joint_angles) 77 | traj.append_waypoint(waypoint.to_msg()) 78 | 79 | result = traj.send_trajectory(timeout=args.timeout) 80 | if result is None: 81 | rospy.logerr('Trajectory FAILED to send') 82 | return 83 | 84 | if result.result: 85 | rospy.loginfo('Motion controller successfully finished the trajectory!') 86 | else: 87 | rospy.logerr('Motion controller failed to complete the trajectory with error %s', 88 | result.errorId) 89 | except rospy.ROSInterruptException: 90 | rospy.logerr('Keyboard interrupt detected from the user. Exiting before trajectory completion.') 91 | 92 | 93 | if __name__ == '__main__': 94 | main() 95 | -------------------------------------------------------------------------------- /intera_examples/scripts/gripper_cuff_control.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | # Copyright (c) 2013-2018, Rethink Robotics Inc. 3 | # 4 | # Licensed under the Apache License, Version 2.0 (the "License"); 5 | # you may not use this file except in compliance with the License. 6 | # You may obtain a copy of the License at 7 | # 8 | # http://www.apache.org/licenses/LICENSE-2.0 9 | # 10 | # Unless required by applicable law or agreed to in writing, software 11 | # distributed under the License is distributed on an "AS IS" BASIS, 12 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | # See the License for the specific language governing permissions and 14 | # limitations under the License. 15 | 16 | import argparse 17 | import sys 18 | 19 | import rospy 20 | 21 | from intera_interface import ( 22 | SimpleClickSmartGripper, 23 | get_current_gripper_interface, 24 | Lights, 25 | Cuff, 26 | RobotParams, 27 | ) 28 | 29 | 30 | class GripperConnect(object): 31 | """ 32 | Connects wrist button presses to gripper open/close commands. 33 | 34 | Uses the Navigator callback feature to make callbacks to connected 35 | action functions when the button values change. 36 | """ 37 | 38 | def __init__(self, arm, lights=True): 39 | """ 40 | @type arm: str 41 | @param arm: arm of gripper to control 42 | @type lights: bool 43 | @param lights: if lights should activate on cuff grasp 44 | """ 45 | self._arm = arm 46 | # inputs 47 | self._cuff = Cuff(limb=arm) 48 | # connect callback fns to signals 49 | self._lights = None 50 | if lights: 51 | self._lights = Lights() 52 | self._cuff.register_callback(self._light_action, 53 | '{0}_cuff'.format(arm)) 54 | try: 55 | self._gripper = get_current_gripper_interface() 56 | self._is_clicksmart = isinstance(self._gripper, SimpleClickSmartGripper) 57 | 58 | if self._is_clicksmart: 59 | if self._gripper.needs_init(): 60 | self._gripper.initialize() 61 | else: 62 | if not (self._gripper.is_calibrated() or 63 | self._gripper.calibrate() == True): 64 | raise 65 | self._cuff.register_callback(self._close_action, 66 | '{0}_button_upper'.format(arm)) 67 | self._cuff.register_callback(self._open_action, 68 | '{0}_button_lower'.format(arm)) 69 | 70 | rospy.loginfo("{0} Cuff Control initialized...".format( 71 | self._gripper.name)) 72 | except: 73 | self._gripper = None 74 | self._is_clicksmart = False 75 | msg = ("{0} Gripper is not connected to the robot." 76 | " Running cuff-light connection only.").format(arm.capitalize()) 77 | rospy.logwarn(msg) 78 | 79 | def _open_action(self, value): 80 | if value and self._gripper.is_ready(): 81 | rospy.logdebug("gripper open triggered") 82 | if self._is_clicksmart: 83 | self._gripper.set_ee_signal_value('grip', False) 84 | else: 85 | self._gripper.open() 86 | if self._lights: 87 | self._set_lights('red', False) 88 | self._set_lights('green', True) 89 | 90 | def _close_action(self, value): 91 | if value and self._gripper.is_ready(): 92 | rospy.logdebug("gripper close triggered") 93 | if self._is_clicksmart: 94 | self._gripper.set_ee_signal_value('grip', True) 95 | else: 96 | self._gripper.close() 97 | if self._lights: 98 | self._set_lights('green', False) 99 | self._set_lights('red', True) 100 | 101 | def _light_action(self, value): 102 | if value: 103 | rospy.logdebug("cuff grasp triggered") 104 | else: 105 | rospy.logdebug("cuff release triggered") 106 | if self._lights: 107 | self._set_lights('red', False) 108 | self._set_lights('green', False) 109 | self._set_lights('blue', value) 110 | 111 | def _set_lights(self, color, value): 112 | self._lights.set_light_state('head_{0}_light'.format(color), on=bool(value)) 113 | self._lights.set_light_state('{0}_hand_{1}_light'.format(self._arm, color), 114 | on=bool(value)) 115 | 116 | def main(): 117 | """SDK Gripper Button Control Example 118 | 119 | Connects cuff buttons to gripper open/close commands: 120 | 'Circle' Button - open gripper 121 | 'Dash' Button - close gripper 122 | Cuff 'Squeeze' - turn on Nav lights 123 | 124 | Run this example in the background or in another terminal 125 | to be able to easily control the grippers by hand while 126 | using the robot. Can be run in parallel with other code. 127 | """ 128 | rp = RobotParams() 129 | valid_limbs = rp.get_limb_names() 130 | if not valid_limbs: 131 | rp.log_message(("Cannot detect any limb parameters on this robot. " 132 | "Exiting."), "ERROR") 133 | return 134 | if len(valid_limbs) > 1: 135 | valid_limbs.append("all_limbs") 136 | arg_fmt = argparse.RawDescriptionHelpFormatter 137 | parser = argparse.ArgumentParser(formatter_class=arg_fmt, 138 | description=main.__doc__) 139 | parser.add_argument('-g', '--gripper', dest='gripper', default=valid_limbs[0], 140 | choices=[valid_limbs], 141 | help='gripper limb to control (default: both)') 142 | parser.add_argument('-n', '--no-lights', dest='lights', 143 | action='store_false', 144 | help='do not trigger lights on cuff grasp') 145 | parser.add_argument('-v', '--verbose', dest='verbosity', 146 | action='store_const', const=rospy.DEBUG, 147 | default=rospy.INFO, 148 | help='print debug statements') 149 | args = parser.parse_args(rospy.myargv()[1:]) 150 | 151 | rospy.init_node('sdk_gripper_cuff_control_{0}'.format(args.gripper), 152 | log_level=args.verbosity) 153 | 154 | arms = (args.gripper,) if args.gripper != 'all_limbs' else valid_limbs[:-1] 155 | grip_ctrls = [GripperConnect(arm, args.lights) for arm in arms] 156 | 157 | print("Press cuff buttons for gripper control. Spinning...") 158 | rospy.spin() 159 | print("Gripper Button Control Finished.") 160 | return 0 161 | 162 | if __name__ == '__main__': 163 | sys.exit(main()) 164 | -------------------------------------------------------------------------------- /intera_examples/scripts/gripper_keyboard.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | # Copyright (c) 2013-2018, Rethink Robotics Inc. 3 | # 4 | # Licensed under the Apache License, Version 2.0 (the "License"); 5 | # you may not use this file except in compliance with the License. 6 | # You may obtain a copy of the License at 7 | # 8 | # http://www.apache.org/licenses/LICENSE-2.0 9 | # 10 | # Unless required by applicable law or agreed to in writing, software 11 | # distributed under the License is distributed on an "AS IS" BASIS, 12 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | # See the License for the specific language governing permissions and 14 | # limitations under the License. 15 | 16 | """ 17 | SDK Gripper Example: keyboard 18 | """ 19 | import argparse 20 | 21 | import rospy 22 | 23 | import intera_interface 24 | import intera_external_devices 25 | from intera_interface import CHECK_VERSION 26 | 27 | def map_keyboard(limb): 28 | # initialize interfaces 29 | print("Getting robot state...") 30 | rs = intera_interface.RobotEnable(CHECK_VERSION) 31 | init_state = rs.state() 32 | gripper = None 33 | original_deadzone = None 34 | def clean_shutdown(): 35 | if gripper and original_deadzone: 36 | gripper.set_dead_zone(original_deadzone) 37 | print("Exiting example.") 38 | try: 39 | gripper = intera_interface.Gripper(limb + '_gripper') 40 | except (ValueError, OSError) as e: 41 | rospy.logerr("Could not detect an electric gripper attached to the robot.") 42 | clean_shutdown() 43 | return 44 | rospy.on_shutdown(clean_shutdown) 45 | 46 | def offset_position(offset_pos): 47 | cmd_pos = max(min(gripper.get_position() + offset_pos, gripper.MAX_POSITION), gripper.MIN_POSITION) 48 | gripper.set_position(cmd_pos) 49 | print("commanded position set to {0} m".format(cmd_pos)) 50 | 51 | def update_velocity(offset_vel): 52 | cmd_speed = max(min(gripper.get_cmd_velocity() + offset_vel, gripper.MAX_VELOCITY), gripper.MIN_VELOCITY) 53 | gripper.set_cmd_velocity(cmd_speed) 54 | print("commanded velocity set to {0} m/s".format(cmd_speed)) 55 | 56 | 57 | original_deadzone = gripper.get_dead_zone() 58 | # WARNING: setting the deadzone below this can cause oscillations in 59 | # the gripper position. However, setting the deadzone to this 60 | # value is required to achieve the incremental commands in this example 61 | gripper.set_dead_zone(0.001) 62 | rospy.loginfo("Gripper deadzone set to {}".format(gripper.get_dead_zone())) 63 | num_steps = 8.0 64 | percent_delta = 1.0 / num_steps 65 | velocity_increment = (gripper.MAX_VELOCITY - gripper.MIN_VELOCITY) * percent_delta 66 | position_increment = (gripper.MAX_POSITION - gripper.MIN_POSITION) * percent_delta 67 | bindings = { 68 | # key: (function, args, description) 69 | 'r': (gripper.reboot, [], "reboot"), 70 | 'c': (gripper.calibrate, [], "calibrate"), 71 | 'q': (gripper.close, [], "close"), 72 | 'o': (gripper.open, [], "open"), 73 | '+': (update_velocity, [velocity_increment], "increase velocity by {0}%".format(percent_delta*100)), 74 | '-': (update_velocity, [-velocity_increment],"decrease velocity by {0}%".format(percent_delta*100)), 75 | 's': (gripper.stop, [], "stop"), 76 | 'u': (offset_position, [-position_increment], "decrease position by {0}%".format(percent_delta*100)), 77 | 'i': (offset_position, [position_increment], "increase position by {0}%".format(percent_delta*100)), 78 | } 79 | 80 | done = False 81 | rospy.loginfo("Enabling robot...") 82 | rs.enable() 83 | print("Controlling grippers. Press ? for help, Esc to quit.") 84 | while not done and not rospy.is_shutdown(): 85 | c = intera_external_devices.getch() 86 | if c: 87 | if c in ['\x1b', '\x03']: 88 | done = True 89 | elif c in bindings: 90 | cmd = bindings[c] 91 | print("command: {0}".format(cmd[2])) 92 | cmd[0](*cmd[1]) 93 | else: 94 | print("key bindings: ") 95 | print(" Esc: Quit") 96 | print(" ?: Help") 97 | for key, val in sorted(list(bindings.items()), 98 | key=lambda x: x[1][2]): 99 | print(" %s: %s" % (key, val[2])) 100 | # force shutdown call if caught by key handler 101 | rospy.signal_shutdown("Example finished.") 102 | 103 | 104 | def main(): 105 | """RSDK Gripper Example: Keyboard Control 106 | 107 | Use your dev machine's keyboard to control and configure grippers. 108 | 109 | Run this example to command various gripper movements while 110 | adjusting gripper parameters, including calibration, and velocity: 111 | Uses the intera_interface.Gripper class and the 112 | helper function, intera_external_devices.getch. 113 | """ 114 | epilog = """ 115 | See help inside the example with the '?' key for key bindings. 116 | """ 117 | rp = intera_interface.RobotParams() 118 | valid_limbs = rp.get_limb_names() 119 | if not valid_limbs: 120 | rp.log_message(("Cannot detect any limb parameters on this robot. " 121 | "Exiting."), "ERROR") 122 | return 123 | arg_fmt = argparse.RawDescriptionHelpFormatter 124 | parser = argparse.ArgumentParser(formatter_class=arg_fmt, 125 | description=main.__doc__, 126 | epilog=epilog) 127 | parser.add_argument( 128 | "-l", "--limb", dest="limb", default=valid_limbs[0], 129 | choices=valid_limbs, 130 | help="Limb on which to run the gripper keyboard example" 131 | ) 132 | args = parser.parse_args(rospy.myargv()[1:]) 133 | 134 | print("Initializing node... ") 135 | rospy.init_node("sdk_gripper_keyboard") 136 | 137 | map_keyboard(args.limb) 138 | 139 | 140 | if __name__ == '__main__': 141 | main() 142 | -------------------------------------------------------------------------------- /intera_examples/scripts/head_display_image.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | # Copyright (c) 2013-2018, Rethink Robotics Inc. 3 | # 4 | # Licensed under the Apache License, Version 2.0 (the "License"); 5 | # you may not use this file except in compliance with the License. 6 | # You may obtain a copy of the License at 7 | # 8 | # http://www.apache.org/licenses/LICENSE-2.0 9 | # 10 | # Unless required by applicable law or agreed to in writing, software 11 | # distributed under the License is distributed on an "AS IS" BASIS, 12 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | # See the License for the specific language governing permissions and 14 | # limitations under the License. 15 | 16 | import intera_interface 17 | import argparse 18 | import rospy 19 | 20 | def main(): 21 | """RSDK Head Display Example: 22 | 23 | Displays a given image file or multiple files on the robot's face. 24 | 25 | Pass the relative or absolute file path to an image file on your 26 | computer, and the example will read and convert the image using 27 | cv_bridge, sending it to the screen as a standard ROS Image Message. 28 | """ 29 | epilog = """ 30 | Notes: 31 | Max screen resolution is 1024x600. 32 | Images are always aligned to the top-left corner. 33 | Image formats are those supported by OpenCv - LoadImage(). 34 | """ 35 | arg_fmt = argparse.RawDescriptionHelpFormatter 36 | parser = argparse.ArgumentParser(formatter_class=arg_fmt, 37 | description=main.__doc__, 38 | epilog=epilog) 39 | required = parser.add_argument_group('required arguments') 40 | required.add_argument( 41 | '-f', '--file', nargs='+', 42 | help='Path to image file to send. Multiple files are separated by a space, eg.: a.png b.png' 43 | ) 44 | parser.add_argument( 45 | '-l', '--loop', action="store_true", 46 | help='Display images in loop, add argument will display images in loop' 47 | ) 48 | parser.add_argument( 49 | '-r', '--rate', type=float, default=1.0, 50 | help='Image display frequency for multiple and looped images.' 51 | ) 52 | args = parser.parse_args() 53 | 54 | rospy.init_node("head_display_example", anonymous=True) 55 | 56 | head_display = intera_interface.HeadDisplay() 57 | head_display.display_image(args.file, args.loop, args.rate) 58 | 59 | if __name__ == '__main__': 60 | main() 61 | -------------------------------------------------------------------------------- /intera_examples/scripts/head_wobbler.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | # Copyright (c) 2013-2018, Rethink Robotics Inc. 3 | # 4 | # Licensed under the Apache License, Version 2.0 (the "License"); 5 | # you may not use this file except in compliance with the License. 6 | # You may obtain a copy of the License at 7 | # 8 | # http://www.apache.org/licenses/LICENSE-2.0 9 | # 10 | # Unless required by applicable law or agreed to in writing, software 11 | # distributed under the License is distributed on an "AS IS" BASIS, 12 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | # See the License for the specific language governing permissions and 14 | # limitations under the License. 15 | 16 | import argparse 17 | import random 18 | 19 | import rospy 20 | 21 | import intera_interface 22 | 23 | from intera_interface import CHECK_VERSION 24 | 25 | 26 | class Wobbler(object): 27 | 28 | def __init__(self): 29 | """ 30 | 'Wobbles' the head 31 | """ 32 | self._done = False 33 | self._head = intera_interface.Head() 34 | 35 | # verify robot is enabled 36 | print("Getting robot state... ") 37 | self._rs = intera_interface.RobotEnable(CHECK_VERSION) 38 | self._init_state = self._rs.state().enabled 39 | print("Enabling robot... ") 40 | self._rs.enable() 41 | print("Running. Ctrl-c to quit") 42 | 43 | def clean_shutdown(self): 44 | """ 45 | Exits example cleanly by moving head to neutral position and 46 | maintaining start state 47 | """ 48 | print("\nExiting example...") 49 | if self._done: 50 | self.set_neutral() 51 | 52 | def set_neutral(self): 53 | """ 54 | Sets the head back into a neutral pose 55 | """ 56 | self._head.set_pan(0.0) 57 | 58 | def wobble(self): 59 | self.set_neutral() 60 | """ 61 | Performs the wobbling 62 | """ 63 | command_rate = rospy.Rate(1) 64 | control_rate = rospy.Rate(100) 65 | start = rospy.get_time() 66 | while not rospy.is_shutdown() and (rospy.get_time() - start < 10.0): 67 | angle = random.uniform(-2.0, 0.95) 68 | while (not rospy.is_shutdown() and 69 | not (abs(self._head.pan() - angle) <= 70 | intera_interface.HEAD_PAN_ANGLE_TOLERANCE)): 71 | self._head.set_pan(angle, speed=0.3, timeout=0) 72 | control_rate.sleep() 73 | command_rate.sleep() 74 | 75 | self._done = True 76 | rospy.signal_shutdown("Example finished.") 77 | 78 | 79 | def main(): 80 | """RSDK Head Example: Wobbler 81 | 82 | Nods the head and pans side-to-side towards random angles. 83 | Demonstrates the use of the intera_interface.Head class. 84 | """ 85 | arg_fmt = argparse.RawDescriptionHelpFormatter 86 | parser = argparse.ArgumentParser(formatter_class=arg_fmt, 87 | description=main.__doc__) 88 | parser.parse_args(rospy.myargv()[1:]) 89 | 90 | print("Initializing node... ") 91 | rospy.init_node("rsdk_head_wobbler") 92 | 93 | wobbler = Wobbler() 94 | rospy.on_shutdown(wobbler.clean_shutdown) 95 | print("Wobbling... ") 96 | wobbler.wobble() 97 | print("Done.") 98 | 99 | if __name__ == '__main__': 100 | main() 101 | -------------------------------------------------------------------------------- /intera_examples/scripts/ik_service_client.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | # Copyright (c) 2013-2018, Rethink Robotics Inc. 3 | # 4 | # Licensed under the Apache License, Version 2.0 (the "License"); 5 | # you may not use this file except in compliance with the License. 6 | # You may obtain a copy of the License at 7 | # 8 | # http://www.apache.org/licenses/LICENSE-2.0 9 | # 10 | # Unless required by applicable law or agreed to in writing, software 11 | # distributed under the License is distributed on an "AS IS" BASIS, 12 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | # See the License for the specific language governing permissions and 14 | # limitations under the License. 15 | 16 | """ 17 | Intera RSDK Inverse Kinematics Example 18 | """ 19 | import rospy 20 | from geometry_msgs.msg import ( 21 | PoseStamped, 22 | Pose, 23 | Point, 24 | Quaternion, 25 | ) 26 | from std_msgs.msg import Header 27 | from sensor_msgs.msg import JointState 28 | 29 | from intera_core_msgs.srv import ( 30 | SolvePositionIK, 31 | SolvePositionIKRequest, 32 | ) 33 | 34 | def ik_service_client(limb = "right", use_advanced_options = False): 35 | ns = "ExternalTools/" + limb + "/PositionKinematicsNode/IKService" 36 | iksvc = rospy.ServiceProxy(ns, SolvePositionIK) 37 | ikreq = SolvePositionIKRequest() 38 | hdr = Header(stamp=rospy.Time.now(), frame_id='base') 39 | poses = { 40 | 'right': PoseStamped( 41 | header=hdr, 42 | pose=Pose( 43 | position=Point( 44 | x=0.450628752997, 45 | y=0.161615832271, 46 | z=0.217447307078, 47 | ), 48 | orientation=Quaternion( 49 | x=0.704020578925, 50 | y=0.710172716916, 51 | z=0.00244101361829, 52 | w=0.00194372088834, 53 | ), 54 | ), 55 | ), 56 | } 57 | # Add desired pose for inverse kinematics 58 | ikreq.pose_stamp.append(poses[limb]) 59 | # Request inverse kinematics from base to "right_hand" link 60 | ikreq.tip_names.append('right_hand') 61 | 62 | if (use_advanced_options): 63 | # Optional Advanced IK parameters 64 | rospy.loginfo("Running Advanced IK Service Client example.") 65 | # The joint seed is where the IK position solver starts its optimization 66 | ikreq.seed_mode = ikreq.SEED_USER 67 | seed = JointState() 68 | seed.name = ['right_j0', 'right_j1', 'right_j2', 'right_j3', 69 | 'right_j4', 'right_j5', 'right_j6'] 70 | seed.position = [0.7, 0.4, -1.7, 1.4, -1.1, -1.6, -0.4] 71 | ikreq.seed_angles.append(seed) 72 | 73 | # Once the primary IK task is solved, the solver will then try to bias the 74 | # the joint angles toward the goal joint configuration. The null space is 75 | # the extra degrees of freedom the joints can move without affecting the 76 | # primary IK task. 77 | ikreq.use_nullspace_goal.append(True) 78 | # The nullspace goal can either be the full set or subset of joint angles 79 | goal = JointState() 80 | goal.name = ['right_j1', 'right_j2', 'right_j3'] 81 | goal.position = [0.1, -0.3, 0.5] 82 | ikreq.nullspace_goal.append(goal) 83 | # The gain used to bias toward the nullspace goal. Must be [0.0, 1.0] 84 | # If empty, the default gain of 0.4 will be used 85 | ikreq.nullspace_gain.append(0.4) 86 | else: 87 | rospy.loginfo("Running Simple IK Service Client example.") 88 | 89 | try: 90 | rospy.wait_for_service(ns, 5.0) 91 | resp = iksvc(ikreq) 92 | except (rospy.ServiceException, rospy.ROSException) as e: 93 | rospy.logerr("Service call failed: %s" % (e,)) 94 | return False 95 | 96 | # Check if result valid, and type of seed ultimately used to get solution 97 | if (resp.result_type[0] > 0): 98 | seed_str = { 99 | ikreq.SEED_USER: 'User Provided Seed', 100 | ikreq.SEED_CURRENT: 'Current Joint Angles', 101 | ikreq.SEED_NS_MAP: 'Nullspace Setpoints', 102 | }.get(resp.result_type[0], 'None') 103 | rospy.loginfo("SUCCESS - Valid Joint Solution Found from Seed Type: %s" % 104 | (seed_str,)) 105 | # Format solution into Limb API-compatible dictionary 106 | limb_joints = dict(list(zip(resp.joints[0].name, resp.joints[0].position))) 107 | rospy.loginfo("\nIK Joint Solution:\n%s", limb_joints) 108 | rospy.loginfo("------------------") 109 | rospy.loginfo("Response Message:\n%s", resp) 110 | else: 111 | rospy.logerr("INVALID POSE - No Valid Joint Solution Found.") 112 | rospy.logerr("Result Error %d", resp.result_type[0]) 113 | return False 114 | 115 | return True 116 | 117 | 118 | def main(): 119 | """RSDK Inverse Kinematics Example 120 | 121 | A simple example of using the Rethink Inverse Kinematics 122 | Service which returns the joint angles and validity for 123 | a requested Cartesian Pose. 124 | 125 | Run this example, the example will use the default limb 126 | and call the Service with a sample Cartesian 127 | Pose, pre-defined in the example code, printing the 128 | response of whether a valid joint solution was found, 129 | and if so, the corresponding joint angles. 130 | """ 131 | rospy.init_node("rsdk_ik_service_client") 132 | 133 | if ik_service_client(): 134 | rospy.loginfo("Simple IK call passed!") 135 | else: 136 | rospy.logerr("Simple IK call FAILED") 137 | 138 | if ik_service_client(use_advanced_options=True): 139 | rospy.loginfo("Advanced IK call passed!") 140 | else: 141 | rospy.logerr("Advanced IK call FAILED") 142 | 143 | 144 | if __name__ == '__main__': 145 | main() 146 | -------------------------------------------------------------------------------- /intera_examples/scripts/joint_position_file_playback.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | # Copyright (c) 2013-2018, Rethink Robotics Inc. 3 | # 4 | # Licensed under the Apache License, Version 2.0 (the "License"); 5 | # you may not use this file except in compliance with the License. 6 | # You may obtain a copy of the License at 7 | # 8 | # http://www.apache.org/licenses/LICENSE-2.0 9 | # 10 | # Unless required by applicable law or agreed to in writing, software 11 | # distributed under the License is distributed on an "AS IS" BASIS, 12 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | # See the License for the specific language governing permissions and 14 | # limitations under the License. 15 | 16 | """ 17 | SDK Joint Position Example: file playback 18 | """ 19 | import argparse 20 | import sys 21 | import rospy 22 | import intera_interface 23 | from intera_interface import CHECK_VERSION 24 | 25 | 26 | def try_float(x): 27 | try: 28 | return float(x) 29 | except ValueError: 30 | return None 31 | 32 | def clean_line(line, names): 33 | """ 34 | Cleans a single line of recorded joint positions 35 | 36 | @param line: the line described in a list to process 37 | @param names: joint name keys 38 | """ 39 | #convert the line of strings to a float or None 40 | line = [try_float(x) for x in line.rstrip().split(',')] 41 | #zip the values with the joint names 42 | combined = list(zip(names[1:], line[1:])) 43 | #take out any tuples that have a none value 44 | cleaned = [x for x in combined if x[1] is not None] 45 | #convert it to a dictionary with only valid commands 46 | command = dict(cleaned) 47 | right_command = dict((key, command[key]) for key in list(command.keys()) 48 | if key[:-2] == 'right_') 49 | return (command, right_command, line) 50 | 51 | def map_file(filename, limb, loops=1): 52 | """ 53 | Loops through csv file 54 | 55 | @param filename: the file to play 56 | @param loops: number of times to loop 57 | values < 0 mean 'infinite' 58 | 59 | Does not loop indefinitely, but only until the file is read 60 | and processed. Reads each line, split up in columns and 61 | formats each line into a controller command in the form of 62 | name/value pairs. Names come from the column headers 63 | first column is the time stamp 64 | """ 65 | limb_interface = intera_interface.Limb(limb) 66 | has_gripper = False 67 | try: 68 | gripper = intera_interface.Gripper(limb + '_gripper') 69 | except: 70 | has_gripper = False 71 | rospy.loginfo("Could not detect a gripper attached to the robot") 72 | 73 | rate = rospy.Rate(100) 74 | if has_gripper: 75 | if gripper.has_error(): 76 | gripper.reboot() 77 | if not gripper.is_calibrated(): 78 | gripper.calibrate() 79 | 80 | print("Playing back: %s" % (filename,)) 81 | with open(filename, 'r') as f: 82 | lines = f.readlines() 83 | keys = lines[0].rstrip().split(',') 84 | 85 | l = 0 86 | # If specified, repeat the file playback 'loops' number of times 87 | while loops < 1 or l < loops: 88 | i = 0 89 | l += 1 90 | print("Moving to start position...") 91 | _cmd, cmd_start, _raw = clean_line(lines[1], keys) 92 | limb_interface.move_to_joint_positions(cmd_start) 93 | start_time = rospy.get_time() 94 | for values in lines[1:]: 95 | i += 1 96 | loopstr = str(loops) if loops > 0 else "forever" 97 | sys.stdout.write("\r Record %d of %d, loop %d of %s" % 98 | (i, len(lines) - 1, l, loopstr)) 99 | sys.stdout.flush() 100 | cmd, limb_cmd, values = clean_line(values, keys) 101 | #command this set of commands until the next frame 102 | while (rospy.get_time() - start_time) < values[0]: 103 | if rospy.is_shutdown(): 104 | print("\n Aborting - ROS shutdown") 105 | return False 106 | if len(limb_cmd): 107 | limb_interface.set_joint_positions(limb_cmd) 108 | if has_gripper and gripper.name in cmd: 109 | gripper.set_position(cmd[gripper.name]) 110 | rate.sleep() 111 | print() 112 | return True 113 | 114 | def main(): 115 | """RSDK Joint Position Example: File Playback 116 | 117 | Uses Joint Position Control mode to play back a series of 118 | recorded joint and gripper positions. 119 | 120 | Run the joint_recorder.py example first to create a recording 121 | file for use with this example. This example uses position 122 | control to replay the recorded positions in sequence. 123 | 124 | Note: This version of the playback example simply drives the 125 | joints towards the next position at each time stamp. Because 126 | it uses Position Control it will not attempt to adjust the 127 | movement speed to hit set points "on time". 128 | """ 129 | epilog = """ 130 | Related examples: 131 | joint_recorder.py; joint_trajectory_file_playback.py. 132 | """ 133 | rp = intera_interface.RobotParams() 134 | valid_limbs = rp.get_limb_names() 135 | if not valid_limbs: 136 | rp.log_message(("Cannot detect any limb parameters on this robot. " 137 | "Exiting."), "ERROR") 138 | return 139 | arg_fmt = argparse.RawDescriptionHelpFormatter 140 | parser = argparse.ArgumentParser(formatter_class=arg_fmt, 141 | description=main.__doc__, 142 | epilog=epilog) 143 | parser.add_argument( 144 | '-f', '--file', metavar='PATH', required=True, 145 | help='path to input file' 146 | ) 147 | parser.add_argument( 148 | "-a", "--arm", dest="arm", default=valid_limbs[0], 149 | choices=valid_limbs, 150 | help="Arm on which to run the joint position file playback example.") 151 | parser.add_argument( 152 | '-l', '--loops', type=int, default=1, 153 | help='number of times to loop the input file. 0=infinite.' 154 | ) 155 | args = parser.parse_args(rospy.myargv()[1:]) 156 | 157 | print("Initializing node... ") 158 | rospy.init_node("sdk_joint_position_file_playback") 159 | print("Getting robot state... ") 160 | rs = intera_interface.RobotEnable(CHECK_VERSION) 161 | init_state = rs.state().enabled 162 | 163 | def clean_shutdown(): 164 | print("\nExiting example...") 165 | 166 | rospy.on_shutdown(clean_shutdown) 167 | 168 | print("Enabling robot... ") 169 | rs.enable() 170 | 171 | map_file(args.file, args.arm, args.loops) 172 | 173 | 174 | if __name__ == '__main__': 175 | main() 176 | -------------------------------------------------------------------------------- /intera_examples/scripts/joint_position_keyboard.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | # Copyright (c) 2013-2018, Rethink Robotics Inc. 3 | # 4 | # Licensed under the Apache License, Version 2.0 (the "License"); 5 | # you may not use this file except in compliance with the License. 6 | # You may obtain a copy of the License at 7 | # 8 | # http://www.apache.org/licenses/LICENSE-2.0 9 | # 10 | # Unless required by applicable law or agreed to in writing, software 11 | # distributed under the License is distributed on an "AS IS" BASIS, 12 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | # See the License for the specific language governing permissions and 14 | # limitations under the License. 15 | 16 | """ 17 | SDK Joint Position Example: keyboard 18 | """ 19 | import argparse 20 | 21 | import rospy 22 | 23 | import intera_interface 24 | import intera_external_devices 25 | 26 | from intera_interface import CHECK_VERSION 27 | 28 | 29 | def map_keyboard(side): 30 | limb = intera_interface.Limb(side) 31 | 32 | try: 33 | gripper = intera_interface.Gripper(side + '_gripper') 34 | except: 35 | has_gripper = False 36 | rospy.loginfo("The electric gripper is not detected on the robot.") 37 | else: 38 | has_gripper = True 39 | 40 | joints = limb.joint_names() 41 | 42 | def set_j(limb, joint_name, delta): 43 | current_position = limb.joint_angle(joint_name) 44 | joint_command = {joint_name: current_position + delta} 45 | limb.set_joint_positions(joint_command) 46 | 47 | def set_g(action): 48 | if has_gripper: 49 | if action == "close": 50 | gripper.close() 51 | elif action == "open": 52 | gripper.open() 53 | elif action == "calibrate": 54 | gripper.calibrate() 55 | 56 | bindings = { 57 | '1': (set_j, [limb, joints[0], 0.1], joints[0]+" increase"), 58 | 'q': (set_j, [limb, joints[0], -0.1], joints[0]+" decrease"), 59 | '2': (set_j, [limb, joints[1], 0.1], joints[1]+" increase"), 60 | 'w': (set_j, [limb, joints[1], -0.1], joints[1]+" decrease"), 61 | '3': (set_j, [limb, joints[2], 0.1], joints[2]+" increase"), 62 | 'e': (set_j, [limb, joints[2], -0.1], joints[2]+" decrease"), 63 | '4': (set_j, [limb, joints[3], 0.1], joints[3]+" increase"), 64 | 'r': (set_j, [limb, joints[3], -0.1], joints[3]+" decrease"), 65 | '5': (set_j, [limb, joints[4], 0.1], joints[4]+" increase"), 66 | 't': (set_j, [limb, joints[4], -0.1], joints[4]+" decrease"), 67 | '6': (set_j, [limb, joints[5], 0.1], joints[5]+" increase"), 68 | 'y': (set_j, [limb, joints[5], -0.1], joints[5]+" decrease"), 69 | '7': (set_j, [limb, joints[6], 0.1], joints[6]+" increase"), 70 | 'u': (set_j, [limb, joints[6], -0.1], joints[6]+" decrease") 71 | } 72 | if has_gripper: 73 | bindings.update({ 74 | '8': (set_g, "close", side+" gripper close"), 75 | 'i': (set_g, "open", side+" gripper open"), 76 | '9': (set_g, "calibrate", side+" gripper calibrate") 77 | }) 78 | done = False 79 | print("Controlling joints. Press ? for help, Esc to quit.") 80 | while not done and not rospy.is_shutdown(): 81 | c = intera_external_devices.getch() 82 | if c: 83 | #catch Esc or ctrl-c 84 | if c in ['\x1b', '\x03']: 85 | done = True 86 | rospy.signal_shutdown("Example finished.") 87 | elif c in bindings: 88 | cmd = bindings[c] 89 | if c == '8' or c == 'i' or c == '9': 90 | cmd[0](cmd[1]) 91 | print("command: %s" % (cmd[2],)) 92 | else: 93 | #expand binding to something like "set_j(right, 'j0', 0.1)" 94 | cmd[0](*cmd[1]) 95 | print("command: %s" % (cmd[2],)) 96 | else: 97 | print("key bindings: ") 98 | print(" Esc: Quit") 99 | print(" ?: Help") 100 | for key, val in sorted(list(bindings.items()), 101 | key=lambda x: x[1][2]): 102 | print(" %s: %s" % (key, val[2])) 103 | 104 | def main(): 105 | """RSDK Joint Position Example: Keyboard Control 106 | 107 | Use your dev machine's keyboard to control joint positions. 108 | 109 | Each key corresponds to increasing or decreasing the angle 110 | of a joint on Sawyer's arm. The increasing and descreasing 111 | are represented by number key and letter key next to the number. 112 | """ 113 | epilog = """ 114 | See help inside the example with the '?' key for key bindings. 115 | """ 116 | rp = intera_interface.RobotParams() 117 | valid_limbs = rp.get_limb_names() 118 | if not valid_limbs: 119 | rp.log_message(("Cannot detect any limb parameters on this robot. " 120 | "Exiting."), "ERROR") 121 | return 122 | arg_fmt = argparse.RawDescriptionHelpFormatter 123 | parser = argparse.ArgumentParser(formatter_class=arg_fmt, 124 | description=main.__doc__, 125 | epilog=epilog) 126 | parser.add_argument( 127 | "-l", "--limb", dest="limb", default=valid_limbs[0], 128 | choices=valid_limbs, 129 | help="Limb on which to run the joint position keyboard example" 130 | ) 131 | args = parser.parse_args(rospy.myargv()[1:]) 132 | 133 | print("Initializing node... ") 134 | rospy.init_node("sdk_joint_position_keyboard") 135 | print("Getting robot state... ") 136 | rs = intera_interface.RobotEnable(CHECK_VERSION) 137 | init_state = rs.state().enabled 138 | 139 | def clean_shutdown(): 140 | print("\nExiting example.") 141 | 142 | rospy.on_shutdown(clean_shutdown) 143 | 144 | rospy.loginfo("Enabling robot...") 145 | rs.enable() 146 | map_keyboard(args.limb) 147 | print("Done.") 148 | 149 | 150 | if __name__ == '__main__': 151 | main() 152 | -------------------------------------------------------------------------------- /intera_examples/scripts/joint_position_waypoints.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | # Copyright (c) 2013-2018, Rethink Robotics Inc. 3 | # 4 | # Licensed under the Apache License, Version 2.0 (the "License"); 5 | # you may not use this file except in compliance with the License. 6 | # You may obtain a copy of the License at 7 | # 8 | # http://www.apache.org/licenses/LICENSE-2.0 9 | # 10 | # Unless required by applicable law or agreed to in writing, software 11 | # distributed under the License is distributed on an "AS IS" BASIS, 12 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | # See the License for the specific language governing permissions and 14 | # limitations under the License. 15 | 16 | """ 17 | Intera SDK Joint Position Waypoints Example 18 | """ 19 | import argparse 20 | import sys 21 | 22 | import rospy 23 | 24 | import intera_interface 25 | 26 | 27 | class Waypoints(object): 28 | def __init__(self, speed, accuracy, limb="right"): 29 | # Create intera_interface limb instance 30 | self._arm = limb 31 | self._limb = intera_interface.Limb(self._arm) 32 | 33 | # Parameters which will describe joint position moves 34 | self._speed = speed 35 | self._accuracy = accuracy 36 | 37 | # Recorded waypoints 38 | self._waypoints = list() 39 | 40 | # Recording state 41 | self._is_recording = False 42 | 43 | # Verify robot is enabled 44 | print("Getting robot state... ") 45 | self._rs = intera_interface.RobotEnable() 46 | self._init_state = self._rs.state().enabled 47 | print("Enabling robot... ") 48 | self._rs.enable() 49 | 50 | # Create Navigator I/O 51 | self._navigator = intera_interface.Navigator() 52 | 53 | def _record_waypoint(self, value): 54 | """ 55 | Stores joint position waypoints 56 | 57 | Navigator 'OK/Wheel' button callback 58 | """ 59 | if value: 60 | print("Waypoint Recorded") 61 | self._waypoints.append(self._limb.joint_angles()) 62 | 63 | def _stop_recording(self, value): 64 | """ 65 | Sets is_recording to false 66 | 67 | Navigator 'Rethink' button callback 68 | """ 69 | # On navigator Rethink button press, stop recording 70 | if value: 71 | print("Recording Stopped") 72 | self._is_recording = False 73 | 74 | def record(self): 75 | """ 76 | Records joint position waypoints upon each Navigator 'OK/Wheel' button 77 | press. 78 | """ 79 | rospy.loginfo("Waypoint Recording Started") 80 | print("Press Navigator 'OK/Wheel' button to record a new joint " 81 | "joint position waypoint.") 82 | print("Press Navigator 'Rethink' button when finished recording " 83 | "waypoints to begin playback") 84 | # Connect Navigator callbacks 85 | # Navigator scroll wheel button press 86 | ok_id = self._navigator.register_callback(self._record_waypoint, 'right_button_ok') 87 | # Navigator Rethink button press 88 | show_id = self._navigator.register_callback(self._stop_recording, 'right_button_show') 89 | 90 | # Set recording flag 91 | self._is_recording = True 92 | 93 | # Loop until waypoints are done being recorded ('Rethink' Button Press) 94 | while not rospy.is_shutdown() and self._is_recording: 95 | rospy.sleep(1.0) 96 | 97 | # We are now done with the navigator callbacks, disconnecting them 98 | self._navigator.deregister_callback(ok_id) 99 | self._navigator.deregister_callback(show_id) 100 | 101 | def playback(self): 102 | """ 103 | Loops playback of recorded joint position waypoints until program is 104 | exited 105 | """ 106 | rospy.sleep(1.0) 107 | 108 | rospy.loginfo("Waypoint Playback Started") 109 | print(" Press Ctrl-C to stop...") 110 | 111 | # Set joint position speed ratio for execution 112 | self._limb.set_joint_position_speed(self._speed) 113 | 114 | # Loop until program is exited 115 | loop = 0 116 | while not rospy.is_shutdown(): 117 | loop += 1 118 | print("Waypoint playback loop #%d " % (loop,)) 119 | for waypoint in self._waypoints: 120 | if rospy.is_shutdown(): 121 | break 122 | self._limb.move_to_joint_positions(waypoint, timeout=20.0, 123 | threshold=self._accuracy) 124 | # Sleep for a few seconds between playback loops 125 | rospy.sleep(3.0) 126 | 127 | # Set joint position speed back to default 128 | self._limb.set_joint_position_speed(0.3) 129 | 130 | def clean_shutdown(self): 131 | print("\nExiting example...") 132 | return True 133 | 134 | 135 | def main(): 136 | """SDK Joint Position Waypoints Example 137 | 138 | Records joint positions each time the navigator 'OK/wheel' 139 | button is pressed. 140 | Upon pressing the navigator 'Rethink' button, the recorded joint positions 141 | will begin playing back in a loop. 142 | """ 143 | arg_fmt = argparse.RawDescriptionHelpFormatter 144 | parser = argparse.ArgumentParser(formatter_class=arg_fmt, 145 | description=main.__doc__) 146 | parser.add_argument( 147 | '-s', '--speed', default=0.3, type=float, 148 | help='joint position motion speed ratio [0.0-1.0] (default:= 0.3)' 149 | ) 150 | parser.add_argument( 151 | '-a', '--accuracy', 152 | default=intera_interface.settings.JOINT_ANGLE_TOLERANCE, type=float, 153 | help='joint position accuracy (rad) at which waypoints must achieve' 154 | ) 155 | args = parser.parse_args(rospy.myargv()[1:]) 156 | 157 | print("Initializing node... ") 158 | rospy.init_node("sdk_joint_position_waypoints", anonymous=True) 159 | 160 | waypoints = Waypoints(args.speed, args.accuracy) 161 | 162 | # Register clean shutdown 163 | rospy.on_shutdown(waypoints.clean_shutdown) 164 | 165 | # Begin example program 166 | waypoints.record() 167 | waypoints.playback() 168 | 169 | if __name__ == '__main__': 170 | main() 171 | -------------------------------------------------------------------------------- /intera_examples/scripts/joint_recorder.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | # Copyright (c) 2013-2018, Rethink Robotics Inc. 3 | # 4 | # Licensed under the Apache License, Version 2.0 (the "License"); 5 | # you may not use this file except in compliance with the License. 6 | # You may obtain a copy of the License at 7 | # 8 | # http://www.apache.org/licenses/LICENSE-2.0 9 | # 10 | # Unless required by applicable law or agreed to in writing, software 11 | # distributed under the License is distributed on an "AS IS" BASIS, 12 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | # See the License for the specific language governing permissions and 14 | # limitations under the License. 15 | 16 | import argparse 17 | 18 | import rospy 19 | 20 | import intera_interface 21 | from intera_examples import JointRecorder 22 | 23 | from intera_interface import CHECK_VERSION 24 | 25 | 26 | def main(): 27 | """SDK Joint Recorder Example 28 | 29 | Record timestamped joint and gripper positions to a file for 30 | later play back. 31 | 32 | Run this example while moving the robot's arm and gripper 33 | to record a time series of joint and gripper positions to a 34 | new csv file with the provided *filename*. This example can 35 | be run in parallel with any other example or standalone 36 | (moving the arms in zero-g mode while pressing the cuff 37 | buttons to open/close grippers). 38 | 39 | You can later play the movements back using one of the 40 | *_file_playback examples. 41 | """ 42 | epilog = """ 43 | Related examples: 44 | joint_position_file_playback.py; joint_trajectory_file_playback.py. 45 | """ 46 | arg_fmt = argparse.RawDescriptionHelpFormatter 47 | parser = argparse.ArgumentParser(formatter_class=arg_fmt, 48 | description=main.__doc__, 49 | epilog=epilog) 50 | required = parser.add_argument_group('required arguments') 51 | required.add_argument( 52 | '-f', '--file', dest='filename', required=True, 53 | help='the file name to record to' 54 | ) 55 | parser.add_argument( 56 | '-r', '--record-rate', type=int, default=100, metavar='RECORDRATE', 57 | help='rate at which to record (default: 100)' 58 | ) 59 | args = parser.parse_args(rospy.myargv()[1:]) 60 | 61 | print("Initializing node... ") 62 | rospy.init_node("sdk_joint_recorder") 63 | print("Getting robot state... ") 64 | rs = intera_interface.RobotEnable(CHECK_VERSION) 65 | print("Enabling robot... ") 66 | rs.enable() 67 | 68 | recorder = JointRecorder(args.filename, args.record_rate) 69 | rospy.on_shutdown(recorder.stop) 70 | 71 | print("Recording. Press Ctrl-C to stop.") 72 | recorder.record() 73 | 74 | print("\nDone.") 75 | 76 | if __name__ == '__main__': 77 | main() 78 | -------------------------------------------------------------------------------- /intera_examples/scripts/joint_trajectory_client.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | # Copyright (c) 2013-2018, Rethink Robotics Inc. 3 | # 4 | # Licensed under the Apache License, Version 2.0 (the "License"); 5 | # you may not use this file except in compliance with the License. 6 | # You may obtain a copy of the License at 7 | # 8 | # http://www.apache.org/licenses/LICENSE-2.0 9 | # 10 | # Unless required by applicable law or agreed to in writing, software 11 | # distributed under the License is distributed on an "AS IS" BASIS, 12 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | # See the License for the specific language governing permissions and 14 | # limitations under the License. 15 | 16 | """ 17 | Intera SDK Joint Trajectory Action Client Example 18 | """ 19 | import argparse 20 | import sys 21 | 22 | from copy import copy 23 | 24 | import rospy 25 | 26 | import actionlib 27 | 28 | from control_msgs.msg import ( 29 | FollowJointTrajectoryAction, 30 | FollowJointTrajectoryGoal, 31 | ) 32 | from trajectory_msgs.msg import ( 33 | JointTrajectoryPoint, 34 | ) 35 | 36 | import intera_interface 37 | 38 | from intera_interface import CHECK_VERSION 39 | 40 | 41 | class Trajectory(object): 42 | def __init__(self, limb, joint_names): 43 | self._joint_names = joint_names 44 | ns = 'robot/limb/' + limb + '/' 45 | self._client = actionlib.SimpleActionClient( 46 | ns + "follow_joint_trajectory", 47 | FollowJointTrajectoryAction, 48 | ) 49 | self._goal = FollowJointTrajectoryGoal() 50 | self._goal_time_tolerance = rospy.Time(0.1) 51 | self._goal.goal_time_tolerance = self._goal_time_tolerance 52 | server_up = self._client.wait_for_server(timeout=rospy.Duration(10.0)) 53 | if not server_up: 54 | rospy.logerr("Timed out waiting for Joint Trajectory" 55 | " Action Server to connect. Start the action server" 56 | " before running example.") 57 | rospy.signal_shutdown("Timed out waiting for Action Server") 58 | sys.exit(1) 59 | self.clear(limb) 60 | 61 | def add_point(self, positions, time): 62 | point = JointTrajectoryPoint() 63 | point.positions = copy(positions) 64 | point.time_from_start = rospy.Duration(time) 65 | self._goal.trajectory.points.append(point) 66 | 67 | def start(self): 68 | self._goal.trajectory.header.stamp = rospy.Time.now() 69 | self._client.send_goal(self._goal) 70 | 71 | def stop(self): 72 | self._client.cancel_goal() 73 | 74 | def wait(self, timeout=15.0): 75 | self._client.wait_for_result(timeout=rospy.Duration(timeout)) 76 | 77 | def result(self): 78 | return self._client.get_result() 79 | 80 | def clear(self, limb): 81 | self._goal = FollowJointTrajectoryGoal() 82 | self._goal.goal_time_tolerance = self._goal_time_tolerance 83 | self._goal.trajectory.joint_names = self._joint_names 84 | 85 | 86 | def main(): 87 | """SDK Joint Trajectory Example: Simple Action Client 88 | 89 | Creates a client of the Joint Trajectory Action Server 90 | to send commands of standard action type, 91 | control_msgs/FollowJointTrajectoryAction. 92 | 93 | Make sure to start the joint_trajectory_action_server.py 94 | first. Then run this example on a specified limb to 95 | command a short series of trajectory points for the arm 96 | to follow. 97 | """ 98 | rp = intera_interface.RobotParams() 99 | valid_limbs = rp.get_limb_names() 100 | if not valid_limbs: 101 | rp.log_message(("Cannot detect any limb parameters on this robot. " 102 | "Exiting."), "ERROR") 103 | return 104 | 105 | arg_fmt = argparse.RawDescriptionHelpFormatter 106 | parser = argparse.ArgumentParser(formatter_class=arg_fmt, 107 | description=main.__doc__) 108 | parser.add_argument( 109 | '-l', '--limb', choices=valid_limbs, default=valid_limbs[0], 110 | help='send joint trajectory to which limb' 111 | ) 112 | 113 | args = parser.parse_args(rospy.myargv()[1:]) 114 | limb = args.limb 115 | 116 | print("Initializing node... ") 117 | rospy.init_node("sdk_joint_trajectory_client_{0}".format(limb)) 118 | print("Getting robot state... ") 119 | rs = intera_interface.RobotEnable(CHECK_VERSION) 120 | print("Enabling robot... ") 121 | rs.enable() 122 | print("Running. Ctrl-c to quit") 123 | 124 | limb_interface = intera_interface.Limb(limb) 125 | traj = Trajectory(limb, limb_interface.joint_names()) 126 | rospy.on_shutdown(traj.stop) 127 | # Command Current Joint Positions first 128 | limb_interface.move_to_neutral() 129 | current_angles = [limb_interface.joint_angle(joint) for joint in limb_interface.joint_names()] 130 | traj.add_point(current_angles, 0.0) 131 | 132 | p1 = current_angles 133 | n_sec = 1.0 134 | traj.add_point(p1, n_sec) 135 | n_sec += 3.0 136 | traj.add_point([x * 0.6 for x in p1], n_sec) 137 | n_sec += 3.0 138 | traj.add_point([x * 0.3 for x in p1], n_sec) 139 | n_sec += 3.0 140 | traj.add_point([x * 0.6 for x in p1], n_sec) 141 | n_sec += 3.0 142 | traj.add_point([x * 0.3 for x in p1], n_sec) 143 | n_sec += 3.0 144 | traj.add_point([x * 0.6 for x in p1], n_sec) 145 | n_sec += 3.0 146 | traj.add_point([x * 1.0 for x in p1], n_sec) 147 | traj.start() 148 | traj.wait(n_sec) 149 | print("Exiting - Joint Trajectory Action Test Complete") 150 | 151 | if __name__ == "__main__": 152 | main() 153 | -------------------------------------------------------------------------------- /intera_examples/scripts/joint_velocity_wobbler.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | # Copyright (c) 2013-2018, Rethink Robotics Inc. 3 | # 4 | # Licensed under the Apache License, Version 2.0 (the "License"); 5 | # you may not use this file except in compliance with the License. 6 | # You may obtain a copy of the License at 7 | # 8 | # http://www.apache.org/licenses/LICENSE-2.0 9 | # 10 | # Unless required by applicable law or agreed to in writing, software 11 | # distributed under the License is distributed on an "AS IS" BASIS, 12 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | # See the License for the specific language governing permissions and 14 | # limitations under the License. 15 | 16 | import argparse 17 | import math 18 | import random 19 | 20 | import rospy 21 | 22 | from std_msgs.msg import ( 23 | UInt16, 24 | ) 25 | 26 | import intera_interface 27 | 28 | from intera_interface import CHECK_VERSION 29 | 30 | 31 | class Wobbler(object): 32 | 33 | def __init__(self): 34 | """ 35 | 'Wobbles' both arms by commanding joint velocities sinusoidally. 36 | """ 37 | self._pub_rate = rospy.Publisher('robot/joint_state_publish_rate', 38 | UInt16, queue_size=10) 39 | self._right_arm = intera_interface.limb.Limb("right") 40 | self._right_joint_names = self._right_arm.joint_names() 41 | 42 | # control parameters 43 | self._rate = 500.0 # Hz 44 | 45 | print("Getting robot state... ") 46 | self._rs = intera_interface.RobotEnable(CHECK_VERSION) 47 | self._init_state = self._rs.state().enabled 48 | print("Enabling robot... ") 49 | self._rs.enable() 50 | 51 | # set joint state publishing to 500Hz 52 | self._pub_rate.publish(self._rate) 53 | 54 | def _reset_control_modes(self): 55 | rate = rospy.Rate(self._rate) 56 | for _ in range(100): 57 | if rospy.is_shutdown(): 58 | return False 59 | self._right_arm.exit_control_mode() 60 | self._pub_rate.publish(100) # 100Hz default joint state rate 61 | rate.sleep() 62 | return True 63 | 64 | def set_neutral(self): 65 | """ 66 | Sets both arms back into a neutral pose. 67 | """ 68 | print("Moving to neutral pose...") 69 | self._right_arm.move_to_neutral() 70 | 71 | def clean_shutdown(self): 72 | print("\nExiting example...") 73 | #return to normal 74 | self._reset_control_modes() 75 | self.set_neutral() 76 | return True 77 | 78 | def wobble(self): 79 | self.set_neutral() 80 | """ 81 | Performs the wobbling of both arms. 82 | """ 83 | rate = rospy.Rate(self._rate) 84 | start = rospy.Time.now() 85 | 86 | def make_v_func(): 87 | """ 88 | returns a randomly parameterized cosine function to control a 89 | specific joint. 90 | """ 91 | period_factor = random.uniform(0.3, 0.5) 92 | amplitude_factor = random.uniform(0.1, 0.2) 93 | 94 | def v_func(elapsed): 95 | w = period_factor * elapsed.to_sec() 96 | return amplitude_factor * math.cos(w * 2 * math.pi) 97 | return v_func 98 | 99 | v_funcs = [make_v_func() for _ in self._right_joint_names] 100 | 101 | def make_cmd(joint_names, elapsed): 102 | return dict([(joint, v_funcs[i](elapsed)) 103 | for i, joint in enumerate(joint_names)]) 104 | 105 | print("Wobbling. Press Ctrl-C to stop...") 106 | while not rospy.is_shutdown(): 107 | self._pub_rate.publish(int(self._rate)) 108 | elapsed = rospy.Time.now() - start 109 | cmd = make_cmd(self._right_joint_names, elapsed) 110 | self._right_arm.set_joint_velocities(cmd) 111 | rate.sleep() 112 | 113 | 114 | def main(): 115 | """Intera RSDK Joint Velocity Example: Wobbler 116 | 117 | Commands joint velocities of randomly parameterized cosine waves 118 | to each joint. Demonstrates Joint Velocity Control Mode. 119 | """ 120 | arg_fmt = argparse.RawDescriptionHelpFormatter 121 | parser = argparse.ArgumentParser(formatter_class=arg_fmt, 122 | description=main.__doc__) 123 | parser.parse_args(rospy.myargv()[1:]) 124 | 125 | print("Initializing node... ") 126 | rospy.init_node("rsdk_joint_velocity_wobbler") 127 | 128 | wobbler = Wobbler() 129 | rospy.on_shutdown(wobbler.clean_shutdown) 130 | wobbler.wobble() 131 | 132 | print("Done.") 133 | 134 | if __name__ == '__main__': 135 | main() 136 | -------------------------------------------------------------------------------- /intera_examples/scripts/lights_blink.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | # Copyright (c) 2013-2018, Rethink Robotics Inc. 3 | # 4 | # Licensed under the Apache License, Version 2.0 (the "License"); 5 | # you may not use this file except in compliance with the License. 6 | # You may obtain a copy of the License at 7 | # 8 | # http://www.apache.org/licenses/LICENSE-2.0 9 | # 10 | # Unless required by applicable law or agreed to in writing, software 11 | # distributed under the License is distributed on an "AS IS" BASIS, 12 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | # See the License for the specific language governing permissions and 14 | # limitations under the License. 15 | 16 | import argparse 17 | 18 | import rospy 19 | 20 | from intera_interface import Lights 21 | 22 | 23 | def test_light_interface(light_name='head_green_light'): 24 | """Blinks a desired Light on then off.""" 25 | l = Lights() 26 | rospy.loginfo("All available lights on this robot:\n{0}\n".format( 27 | ', '.join(l.list_all_lights()))) 28 | rospy.loginfo("Blinking Light: {0}".format(light_name)) 29 | initial_state = l.get_light_state(light_name) 30 | on_off = lambda x: 'ON' if l.get_light_state(x) else 'OFF' 31 | rospy.loginfo("Initial state: {0}".format(on_off(light_name))) 32 | # invert light 33 | state = not initial_state 34 | l.set_light_state(light_name, state) 35 | rospy.sleep(1) 36 | rospy.loginfo("New state: {0}".format(on_off(light_name))) 37 | # invert light 38 | state = not state 39 | l.set_light_state(light_name, state) 40 | rospy.sleep(1) 41 | rospy.loginfo("New state: {0}".format(on_off(light_name))) 42 | # invert light 43 | state = not state 44 | l.set_light_state(light_name, state) 45 | rospy.sleep(1) 46 | rospy.loginfo("New state: {0}".format(on_off(light_name))) 47 | # reset output 48 | l.set_light_state(light_name, initial_state) 49 | rospy.sleep(1) 50 | rospy.loginfo("Final state: {0}".format(on_off(light_name))) 51 | 52 | 53 | def main(): 54 | """Intera SDK Lights Example: Blink 55 | 56 | Toggles the Lights interface on then off again 57 | while printing the state at each step. Simple demonstration 58 | of using the intera_interface.Lights class. 59 | 60 | Run this example with default arguments and watch the green 61 | light on the head blink on and off while the console 62 | echos the state. Use the light names from Lights.list_all_lights() 63 | to change lights to toggle. 64 | """ 65 | epilog = """ Intera Interface Lights """ 66 | arg_fmt = argparse.RawDescriptionHelpFormatter 67 | parser = argparse.ArgumentParser(formatter_class=arg_fmt, 68 | description=main.__doc__, 69 | epilog=epilog) 70 | parser.add_argument( 71 | '-l', '--light_name', dest='light_name', 72 | default='head_green_light', 73 | help=('name of Light component to use' 74 | ' (default: head_green_light)') 75 | ) 76 | args = parser.parse_args(rospy.myargv()[1:]) 77 | 78 | rospy.init_node('sdk_lights_blink', anonymous=True) 79 | test_light_interface(args.light_name) 80 | 81 | if __name__ == '__main__': 82 | main() 83 | -------------------------------------------------------------------------------- /intera_examples/scripts/navigator_io.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | # Copyright (c) 2013-2018, Rethink Robotics Inc. 3 | # 4 | # Licensed under the Apache License, Version 2.0 (the "License"); 5 | # you may not use this file except in compliance with the License. 6 | # You may obtain a copy of the License at 7 | # 8 | # http://www.apache.org/licenses/LICENSE-2.0 9 | # 10 | # Unless required by applicable law or agreed to in writing, software 11 | # distributed under the License is distributed on an "AS IS" BASIS, 12 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | # See the License for the specific language governing permissions and 14 | # limitations under the License. 15 | 16 | import argparse 17 | import sys 18 | 19 | import rospy 20 | 21 | import intera_interface 22 | 23 | 24 | def echo_input(nav_name='right'): 25 | nav = intera_interface.Navigator() 26 | def back_pressed(v): 27 | rospy.loginfo("Button 'Back': {0}".format(nav.button_string_lookup(v))) 28 | 29 | def rethink_pressed(v): 30 | rospy.loginfo ("Button 'Rethink': {0}".format(nav.button_string_lookup(v))) 31 | 32 | def circle_pressed(v): 33 | rospy.loginfo ("Button 'Circle': {0}".format(nav.button_string_lookup(v))) 34 | 35 | def square_pressed(v): 36 | rospy.loginfo ("Button 'Square': {0}".format(nav.button_string_lookup(v))) 37 | 38 | def x_pressed(v): 39 | rospy.loginfo ("Button 'X': {0}".format(nav.button_string_lookup(v))) 40 | 41 | def ok_pressed(v): 42 | rospy.loginfo ("Button 'OK': {0}".format(nav.button_string_lookup(v))) 43 | 44 | def wheel_moved(v): 45 | rospy.loginfo ("Wheel value: {0}".format(v)) 46 | 47 | nav.register_callback(back_pressed, '_'.join([nav_name, 'button_back'])) 48 | nav.register_callback(rethink_pressed, '_'.join([nav_name, 'button_show'])) 49 | nav.register_callback(circle_pressed, '_'.join([nav_name, 'button_circle'])) 50 | nav.register_callback(square_pressed, '_'.join([nav_name, 'button_square'])) 51 | nav.register_callback(x_pressed, '_'.join([nav_name, 'button_triangle'])) 52 | nav.register_callback(ok_pressed, '_'.join([nav_name, 'button_ok'])) 53 | nav.register_callback(wheel_moved, '_'.join([nav_name, 'wheel'])) 54 | 55 | print ("Press input buttons on the right navigator, " 56 | "input will be echoed here.") 57 | 58 | rate = rospy.Rate(10) 59 | i = 0 60 | while not rospy.is_shutdown() and i < 100: 61 | rate.sleep() 62 | i += 1 63 | 64 | 65 | def main(): 66 | """SDK Navigator Example 67 | 68 | Demonstrates Navigator by echoing input values from wheels and 69 | buttons. 70 | 71 | Uses the intera_interface.Navigator class to demonstrate an 72 | example of using the register_callback feature. 73 | 74 | Shows Navigator input of the arm for 10 seconds. 75 | """ 76 | arg_fmt = argparse.ArgumentDefaultsHelpFormatter 77 | parser = argparse.ArgumentParser(formatter_class=arg_fmt) 78 | parser.add_argument( 79 | "-n", "--navigator", dest="nav_name", default="right", 80 | choices=["right", "head"], 81 | help='Navigator on which to run example' 82 | ) 83 | args = parser.parse_args(rospy.myargv()[1:]) 84 | 85 | rospy.init_node('sdk_navigator', anonymous=True) 86 | echo_input(args.nav_name) 87 | return 0 88 | 89 | if __name__ == '__main__': 90 | sys.exit(main()) 91 | -------------------------------------------------------------------------------- /intera_examples/scripts/send_traj_from_file.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | # Copyright (c) 2016-2018, Rethink Robotics Inc. 3 | # 4 | # Licensed under the Apache License, Version 2.0 (the "License"); 5 | # you may not use this file except in compliance with the License. 6 | # You may obtain a copy of the License at 7 | # 8 | # http://www.apache.org/licenses/LICENSE-2.0 9 | # 10 | # Unless required by applicable law or agreed to in writing, software 11 | # distributed under the License is distributed on an "AS IS" BASIS, 12 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | # See the License for the specific language governing permissions and 14 | # limitations under the License. 15 | 16 | 17 | import rospy 18 | import argparse 19 | from intera_motion_interface import ( 20 | MotionTrajectory, 21 | MotionWaypoint, 22 | MotionWaypointOptions 23 | ) 24 | from intera_interface import Limb 25 | 26 | def main(): 27 | """ 28 | This is an example script to demonstrate the functionality of the MotionTrajectory 29 | to read a yaml file. This script reads in the yaml file creates a trajectory object 30 | and sends the trajectory to the robot. 31 | """ 32 | arg_fmt = argparse.RawDescriptionHelpFormatter 33 | parser = argparse.ArgumentParser(formatter_class=arg_fmt, 34 | description=main.__doc__) 35 | 36 | parser.add_argument( 37 | "-f", "--file_name", type=str, required=True, 38 | help="The name of the yaml file to ready the trajectory from") 39 | parser.add_argument( 40 | "--timeout", type=float, default=None, 41 | help="Max time in seconds to complete motion goal before returning. None is interpreted as an infinite timeout.") 42 | 43 | args = parser.parse_args(rospy.myargv()[1:]) 44 | 45 | 46 | try: 47 | 48 | rospy.init_node('send_traj_from_file') 49 | limb = Limb() 50 | traj = MotionTrajectory(limb = limb) 51 | 52 | read_result=traj.load_yaml_file(args.file_name) 53 | 54 | if not read_result: 55 | rospy.logerr('Trajectory not successfully read from file') 56 | 57 | result = traj.send_trajectory(timeout=args.timeout) 58 | if result is None: 59 | rospy.logerr('Trajectory FAILED to send') 60 | return 61 | 62 | if result.result: 63 | rospy.loginfo('Motion controller successfully finished the trajectory!') 64 | else: 65 | rospy.logerr('Motion controller failed to complete the trajectory with error %s', 66 | result.errorId) 67 | except rospy.ROSInterruptException: 68 | rospy.logerr('Keyboard interrupt detected from the user. Exiting before trajectory completion.') 69 | 70 | 71 | if __name__ == '__main__': 72 | main() 73 | -------------------------------------------------------------------------------- /intera_examples/scripts/stop_motion_trajectory.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | # Copyright (c) 2017, Rethink Robotics Inc. 3 | # 4 | # Licensed under the Apache License, Version 2.0 (the "License"); 5 | # you may not use this file except in compliance with the License. 6 | # You may obtain a copy of the License at 7 | # 8 | # http://www.apache.org/licenses/LICENSE-2.0 9 | # 10 | # Unless required by applicable law or agreed to in writing, software 11 | # distributed under the License is distributed on an "AS IS" BASIS, 12 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | # See the License for the specific language governing permissions and 14 | # limitations under the License. 15 | 16 | import rospy 17 | from intera_motion_interface import MotionTrajectory 18 | 19 | def main(): 20 | """ 21 | Send a STOP command to the motion controller, which will safely stop the 22 | motion controller if it is actively running a trajectory. This is useful 23 | when the robot is executing a long trajectory that needs to be canceled. 24 | Note: This will only stop motions that are running through the motion 25 | controller. It will not stop the motor controllers from receiving commands 26 | send directly from a custom ROS node. 27 | 28 | $ rosrun intera_examples stop_motion_trajectory.py 29 | """ 30 | 31 | try: 32 | rospy.init_node('stop_motion_trajectory') 33 | traj = MotionTrajectory() 34 | result = traj.stop_trajectory() 35 | 36 | if result is None: 37 | rospy.logerr('FAILED to send stop request') 38 | return 39 | 40 | if result.result: 41 | rospy.loginfo('Motion controller successfully stopped the motion!') 42 | else: 43 | rospy.logerr('Motion controller failed to stop the motion: %s', 44 | result.errorId) 45 | 46 | except rospy.ROSInterruptException: 47 | rospy.logerr('Keyboard interrupt detected from the user. Exiting before stop completion.') 48 | 49 | 50 | if __name__ == '__main__': 51 | main() 52 | -------------------------------------------------------------------------------- /intera_examples/setup.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | from setuptools import setup 3 | from catkin_pkg.python_setup import generate_distutils_setup 4 | 5 | d = generate_distutils_setup() 6 | d['packages'] = ['intera_examples', 'intera_external_devices'] 7 | d['package_dir'] = {'': 'src'} 8 | 9 | setup(**d) 10 | -------------------------------------------------------------------------------- /intera_examples/share/images/sawyer_and_baxter.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RethinkRobotics/intera_sdk/6614dec1c5c2e7a74db1af6d01811d1332801785/intera_examples/share/images/sawyer_and_baxter.png -------------------------------------------------------------------------------- /intera_examples/share/images/sawyer_sdk_research.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RethinkRobotics/intera_sdk/6614dec1c5c2e7a74db1af6d01811d1332801785/intera_examples/share/images/sawyer_sdk_research.png -------------------------------------------------------------------------------- /intera_examples/src/intera_examples/__init__.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2013-2018, Rethink Robotics Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from .recorder import JointRecorder 16 | -------------------------------------------------------------------------------- /intera_examples/src/intera_examples/recorder.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2013-2018, Rethink Robotics Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | import rospy 16 | 17 | import intera_interface 18 | 19 | from intera_interface import CHECK_VERSION 20 | 21 | 22 | class JointRecorder(object): 23 | def __init__(self, filename, rate, side="right"): 24 | """ 25 | Records joint data to a file at a specified rate. 26 | """ 27 | self.gripper_name = '_'.join([side, 'gripper']) 28 | self._filename = filename 29 | self._raw_rate = rate 30 | self._rate = rospy.Rate(rate) 31 | self._start_time = rospy.get_time() 32 | self._done = False 33 | 34 | self._limb_right = intera_interface.Limb(side) 35 | try: 36 | self._gripper = intera_interface.Gripper(self.gripper_name) 37 | rospy.loginfo("Electric gripper detected.") 38 | except Exception as e: 39 | self._gripper = None 40 | rospy.loginfo("No electric gripper detected.") 41 | 42 | # Verify Gripper Have No Errors and are Calibrated 43 | if self._gripper: 44 | if self._gripper.has_error(): 45 | self._gripper.reboot() 46 | if not self._gripper.is_calibrated(): 47 | self._gripper.calibrate() 48 | 49 | self._cuff = intera_interface.Cuff(side) 50 | 51 | def _time_stamp(self): 52 | return rospy.get_time() - self._start_time 53 | 54 | def stop(self): 55 | """ 56 | Stop recording. 57 | """ 58 | self._done = True 59 | 60 | def done(self): 61 | """ 62 | Return whether or not recording is done. 63 | """ 64 | if rospy.is_shutdown(): 65 | self.stop() 66 | return self._done 67 | 68 | def record(self): 69 | """ 70 | Records the current joint positions to a csv file if outputFilename was 71 | provided at construction this function will record the latest set of 72 | joint angles in a csv format. 73 | 74 | If a file exists, the function will overwrite existing file. 75 | """ 76 | if self._filename: 77 | joints_right = self._limb_right.joint_names() 78 | with open(self._filename, 'w') as f: 79 | f.write('time,') 80 | temp_str = '' if self._gripper else '\n' 81 | f.write(','.join([j for j in joints_right]) + ',' + temp_str) 82 | if self._gripper: 83 | f.write(self.gripper_name+'\n') 84 | while not self.done(): 85 | if self._gripper: 86 | if self._cuff.upper_button(): 87 | self._gripper.open() 88 | elif self._cuff.lower_button(): 89 | self._gripper.close() 90 | angles_right = [self._limb_right.joint_angle(j) 91 | for j in joints_right] 92 | f.write("%f," % (self._time_stamp(),)) 93 | f.write(','.join([str(x) for x in angles_right]) + ',' + temp_str) 94 | if self._gripper: 95 | f.write(str(self._gripper.get_position()) + '\n') 96 | self._rate.sleep() 97 | -------------------------------------------------------------------------------- /intera_examples/src/intera_external_devices/__init__.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2013-2018, Rethink Robotics Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from .getch import getch 16 | from .joystick import ( 17 | XboxController, 18 | LogitechController, 19 | PS3Controller, 20 | ) 21 | -------------------------------------------------------------------------------- /intera_examples/src/intera_external_devices/getch.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | # Copyright (c) 2013-2018, Rethink Robotics Inc. 3 | # 4 | # Licensed under the Apache License, Version 2.0 (the "License"); 5 | # you may not use this file except in compliance with the License. 6 | # You may obtain a copy of the License at 7 | # 8 | # http://www.apache.org/licenses/LICENSE-2.0 9 | # 10 | # Unless required by applicable law or agreed to in writing, software 11 | # distributed under the License is distributed on an "AS IS" BASIS, 12 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | # See the License for the specific language governing permissions and 14 | # limitations under the License. 15 | 16 | import sys 17 | import termios 18 | import tty 19 | from select import select 20 | 21 | 22 | def getch(timeout=0.01): 23 | """ 24 | Retrieves a character from stdin. 25 | 26 | Returns None if no character is available within the timeout. 27 | Blocks if timeout < 0. 28 | """ 29 | # If this is being piped to, ignore non-blocking functionality 30 | if not sys.stdin.isatty(): 31 | return sys.stdin.read(1) 32 | fileno = sys.stdin.fileno() 33 | old_settings = termios.tcgetattr(fileno) 34 | ch = None 35 | try: 36 | tty.setraw(fileno) 37 | rlist = [fileno] 38 | if timeout >= 0: 39 | [rlist, _, _] = select(rlist, [], [], timeout) 40 | if fileno in rlist: 41 | ch = sys.stdin.read(1) 42 | except Exception as ex: 43 | print("getch", ex) 44 | raise OSError 45 | finally: 46 | termios.tcsetattr(fileno, termios.TCSADRAIN, old_settings) 47 | return ch 48 | -------------------------------------------------------------------------------- /intera_interface/.gitignore: -------------------------------------------------------------------------------- 1 | *.py[cod] 2 | 3 | # C extensions 4 | *.so 5 | 6 | # Packages 7 | *.egg 8 | *.egg-info 9 | dist 10 | build 11 | eggs 12 | parts 13 | bin 14 | var 15 | sdist 16 | develop-eggs 17 | .installed.cfg 18 | lib 19 | lib64 20 | 21 | # Installer logs 22 | pip-log.txt 23 | 24 | # Unit test / coverage reports 25 | .coverage 26 | .tox 27 | nosetests.xml 28 | 29 | # Translations 30 | *.mo 31 | 32 | # Mr Developer 33 | .mr.developer.cfg 34 | .project 35 | .cproject 36 | cmake_install.cmake 37 | .pydevproject 38 | *.swp 39 | *.swo 40 | CATKIN_IGNORE 41 | catkin 42 | catkin_generated 43 | devel 44 | 45 | cpp 46 | docs 47 | msg_gen 48 | srv_gen 49 | *.smoketest 50 | *.cfgc 51 | *_msgs/src/ 52 | */src/**/cfg 53 | */*/src/**/* 54 | -------------------------------------------------------------------------------- /intera_interface/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | 5.0.3 (2016-12-05) 2 | --------------------------------- 3 | - Initial release of intera_interface, borrowing heavily from baxter_interface 4 | but robot-agnostic to any robot running Intera 5 | -------------------------------------------------------------------------------- /intera_interface/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(intera_interface) 3 | 4 | find_package(catkin 5 | REQUIRED 6 | COMPONENTS 7 | cv_bridge 8 | rospy 9 | actionlib 10 | sensor_msgs 11 | std_msgs 12 | control_msgs 13 | trajectory_msgs 14 | dynamic_reconfigure 15 | intera_core_msgs 16 | intera_motion_msgs 17 | ) 18 | 19 | catkin_python_setup() 20 | 21 | generate_dynamic_reconfigure_options( 22 | cfg/SawyerPositionJointTrajectoryActionServer.cfg 23 | cfg/SawyerVelocityJointTrajectoryActionServer.cfg 24 | cfg/SawyerPositionFFJointTrajectoryActionServer.cfg 25 | ) 26 | 27 | add_dependencies(${PROJECT_NAME}_gencfg intera_core_msgs_generate_messages_py) 28 | 29 | catkin_package( 30 | CATKIN_DEPENDS 31 | cv_bridge 32 | rospy 33 | actionlib 34 | sensor_msgs 35 | std_msgs 36 | control_msgs 37 | trajectory_msgs 38 | dynamic_reconfigure 39 | intera_core_msgs 40 | intera_motion_msgs 41 | ) 42 | 43 | catkin_install_python(PROGRAMS 44 | scripts/home_joints.py 45 | scripts/joint_trajectory_action_server.py 46 | scripts/calibrate_arm.py 47 | scripts/io_config_editor.py 48 | scripts/send_urdf_fragment.py 49 | scripts/enable_robot.py 50 | 51 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) 52 | 53 | install( 54 | DIRECTORY scripts/ 55 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 56 | USE_SOURCE_PERMISSIONS 57 | ) 58 | -------------------------------------------------------------------------------- /intera_interface/README.rst: -------------------------------------------------------------------------------- 1 | intera_interface 2 | ================ 3 | 4 | Python interface classes and action servers for control of 5 | the Intera Research Robot from Rethink Robotics 6 | 7 | Code & Tickets 8 | -------------- 9 | 10 | +-----------------+----------------------------------------------------------------+ 11 | | Documentation | http://sdk.rethinkrobotics.com/intera/ | 12 | +-----------------+----------------------------------------------------------------+ 13 | | Issues | https://github.com/RethinkRobotics/intera_interface/issues | 14 | +-----------------+----------------------------------------------------------------+ 15 | | Contributions | http://sdk.rethinkrobotics.com/intera/Contributions | 16 | +-----------------+----------------------------------------------------------------+ 17 | 18 | intera_interface Repository Overview 19 | ------------------------------------ 20 | 21 | :: 22 | 23 | . 24 | | 25 | +-- src/ intera_interface api 26 | | +-- intera_io/ basic interface for IO Framework 27 | | +-- intera_interface/ intera component classes 28 | | +-- camera.py 29 | | +-- cuff.py 30 | | +-- digital_io.py 31 | | +-- gripper.py 32 | | +-- head.py 33 | | +-- head_display.py 34 | | +-- lights.py 35 | | +-- limb.py 36 | | +-- navigator.py 37 | | +-- robot_enable.py 38 | | +-- robot_params.py 39 | | +-- settings.py 40 | | +-- intera_control/ generic control utilities 41 | | +-- intera_dataflow/ timing/program flow utilities 42 | | +-- intera_joint_trajectory_action/ joint trajectory action implementation 43 | | 44 | +-- scripts/ utility executable scripts 45 | | +-- calibrate_arm.py arm calibration action client 46 | | +-- enable_robot.py enable / disable / reset the robot 47 | | +-- home_joints.py script to home the joints on the robot 48 | | +-- joint_trajectory_action_server.py trajectory action server for use with MoveIt 49 | | +-- send_urdf_fragment.py send URDF fragment to update robot's URDF 50 | | 51 | +-- cfg/ dynamic reconfigure action configs 52 | 53 | 54 | Other Intera Repositories 55 | ------------------------- 56 | 57 | +------------------+-----------------------------------------------------+ 58 | | intera_common | https://github.com/RethinkRobotics/intera_common | 59 | +------------------+-----------------------------------------------------+ 60 | 61 | Latest Release Information 62 | -------------------------- 63 | 64 | http://sdk.rethinkrobotics.com/intera/Release-Changes 65 | -------------------------------------------------------------------------------- /intera_interface/cfg/SawyerPositionFFJointTrajectoryActionServer.cfg: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | # Copyright (c) 2013-2018, Rethink Robotics Inc. 3 | # 4 | # Licensed under the Apache License, Version 2.0 (the "License"); 5 | # you may not use this file except in compliance with the License. 6 | # You may obtain a copy of the License at 7 | # 8 | # http://www.apache.org/licenses/LICENSE-2.0 9 | # 10 | # Unless required by applicable law or agreed to in writing, software 11 | # distributed under the License is distributed on an "AS IS" BASIS, 12 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | # See the License for the specific language governing permissions and 14 | # limitations under the License. 15 | 16 | from dynamic_reconfigure.parameter_generator_catkin import ( 17 | ParameterGenerator, 18 | double_t, 19 | ) 20 | 21 | gen = ParameterGenerator() 22 | 23 | gen.add( 24 | 'goal_time', double_t, 0, 25 | "Amount of time (s) controller is permitted to be late achieving goal", 26 | 0.1, 0.0, 120.0, 27 | ) 28 | gen.add( 29 | 'stopped_velocity_tolerance', double_t, 0, 30 | "Maximum velocity (m/s) at end of trajectory to be considered stopped", 31 | 0.20, -1.0, 1.0, 32 | ) 33 | 34 | joints = ('right_j0', 'right_j1', 'right_j2', 'right_j3', 'right_j4','right_j5', 'right_j6',) 35 | 36 | params = ('_goal', '_trajectory',) 37 | msg = ( 38 | " - maximum final error", 39 | " - maximum error during trajectory execution", 40 | ) 41 | min = (-0.5, -0.5,) 42 | default = (-0.2, 0.2,) 43 | max = (0.5, 0.5,) 44 | 45 | for idx, param in enumerate(params): 46 | for joint in joints: 47 | gen.add( 48 | joint + param, double_t, 0, joint + msg[idx], 49 | default[idx], min[idx], max[idx] 50 | ) 51 | 52 | exit(gen.generate('intera_interface', '', 'SawyerPositionFFJointTrajectoryActionServer')) 53 | -------------------------------------------------------------------------------- /intera_interface/cfg/SawyerPositionJointTrajectoryActionServer.cfg: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | # Copyright (c) 2013-2018, Rethink Robotics Inc. 3 | # 4 | # Licensed under the Apache License, Version 2.0 (the "License"); 5 | # you may not use this file except in compliance with the License. 6 | # You may obtain a copy of the License at 7 | # 8 | # http://www.apache.org/licenses/LICENSE-2.0 9 | # 10 | # Unless required by applicable law or agreed to in writing, software 11 | # distributed under the License is distributed on an "AS IS" BASIS, 12 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | # See the License for the specific language governing permissions and 14 | # limitations under the License. 15 | 16 | from dynamic_reconfigure.parameter_generator_catkin import ( 17 | ParameterGenerator, 18 | double_t, 19 | ) 20 | 21 | gen = ParameterGenerator() 22 | 23 | gen.add( 24 | 'goal_time', double_t, 0, 25 | "Amount of time (s) controller is permitted to be late achieving goal", 26 | 0.1, 0.0, 120.0, 27 | ) 28 | gen.add( 29 | 'stopped_velocity_tolerance', double_t, 0, 30 | "Maximum velocity (m/s) at end of trajectory to be considered stopped", 31 | 0.25, -1.0, 1.0, 32 | ) 33 | 34 | joints = ('right_j0', 'right_j1', 'right_j2', 'right_j3', 'right_j4','right_j5', 'right_j6',) 35 | 36 | params = ('_goal', '_trajectory',) 37 | msg = ( 38 | " - maximum final error", 39 | " - maximum error during trajectory execution", 40 | ) 41 | min = (-1.0, -1.0,) 42 | default = (-1.0, 0.2,) 43 | max = (1.0, 1.0,) 44 | 45 | for idx, param in enumerate(params): 46 | for joint in joints: 47 | gen.add( 48 | joint + param, double_t, 0, joint + msg[idx], 49 | default[idx], min[idx], max[idx] 50 | ) 51 | 52 | exit(gen.generate('intera_interface', '', 'SawyerPositionJointTrajectoryActionServer')) 53 | -------------------------------------------------------------------------------- /intera_interface/cfg/SawyerVelocityJointTrajectoryActionServer.cfg: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | # Copyright (c) 2013-2018, Rethink Robotics Inc. 3 | # 4 | # Licensed under the Apache License, Version 2.0 (the "License"); 5 | # you may not use this file except in compliance with the License. 6 | # You may obtain a copy of the License at 7 | # 8 | # http://www.apache.org/licenses/LICENSE-2.0 9 | # 10 | # Unless required by applicable law or agreed to in writing, software 11 | # distributed under the License is distributed on an "AS IS" BASIS, 12 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | # See the License for the specific language governing permissions and 14 | # limitations under the License. 15 | 16 | from dynamic_reconfigure.parameter_generator_catkin import ( 17 | ParameterGenerator, 18 | double_t, 19 | ) 20 | 21 | gen = ParameterGenerator() 22 | 23 | gen.add( 24 | 'goal_time', double_t, 0, 25 | "Amount of time (s) controller is permitted to be late achieving goal", 26 | 0.0, 0.0, 120.0, 27 | ) 28 | 29 | gen.add( 30 | 'stopped_velocity_tolerance', double_t, 0, 31 | "Maximum velocity (m/s) at end of trajectory to be considered stopped", 32 | -1.0, -1.0, 1.0, 33 | ) 34 | 35 | joints = ('right_j0', 'right_j1', 'right_j2', 'right_j3', 'right_j4','right_j5', 'right_j6',) 36 | 37 | params = ('_goal', '_trajectory', '_kp', '_ki', '_kd',) 38 | msg = ( 39 | " - maximum final error", 40 | " - maximum error during trajectory execution", 41 | " - Kp proportional control gain", 42 | " - Ki integral control gain", 43 | " - Kd derivative control gain", 44 | ) 45 | min = (-1.0, -1.0, 0.0, 0.0, 0.0,) 46 | default = (-1.0, -1.0, 2.0, 0.0, 0.0,) 47 | max = (3.0, 3.0, 500.0, 100.0, 100.0,) 48 | 49 | for idx, param in enumerate(params): 50 | if idx < 2: 51 | for joint in joints: 52 | gen.add( 53 | joint + param, double_t, 0, joint + msg[idx], 54 | default[idx], min[idx], max[idx] 55 | ) 56 | for joint in joints: 57 | for idx, param in enumerate(params): 58 | if idx >= 2: 59 | gen.add( 60 | joint + param, double_t, 0, joint + msg[idx], 61 | default[idx], min[idx], max[idx] 62 | ) 63 | 64 | exit(gen.generate('intera_interface', '', 'SawyerVelocityJointTrajectoryActionServer')) 65 | -------------------------------------------------------------------------------- /intera_interface/epydoc.config: -------------------------------------------------------------------------------- 1 | [epydoc] 2 | inheritance: included 3 | url: http://sdk.rethinkrobotics.com/intera/ 4 | imports: yes 5 | modules: intera_interface, intera_dataflow, intera_control, intera_io, intera_motion_interface 6 | top: intera_interface 7 | -------------------------------------------------------------------------------- /intera_interface/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | intera_interface 5 | 5.3.0 6 | 7 | Python interface classes for control of 8 | Intera-enable Robots from Rethink Robotics. 9 | 10 | 11 | 12 | Rethink Robotics Inc. 13 | 14 | Apache 2.0 15 | http://sdk.rethinkrobotics.com/intera/ 16 | 17 | https://github.com/RethinkRobotics/intera_sdk 18 | 19 | 20 | https://github.com/RethinkRobotics/intera_sdk/issues 21 | 22 | Rethink Robotics Inc. 23 | 24 | python-setuptools 25 | python3-setuptools 26 | catkin 27 | cv_bridge 28 | rospy 29 | actionlib 30 | intera_core_msgs 31 | intera_motion_msgs 32 | control_msgs 33 | dynamic_reconfigure 34 | sensor_msgs 35 | std_msgs 36 | trajectory_msgs 37 | 38 | cv_bridge 39 | rospy 40 | actionlib 41 | intera_core_msgs 42 | intera_motion_msgs 43 | control_msgs 44 | dynamic_reconfigure 45 | sensor_msgs 46 | std_msgs 47 | trajectory_msgs 48 | rospy_message_converter 49 | xacro 50 | 51 | 52 | 53 | 54 | 55 | 56 | -------------------------------------------------------------------------------- /intera_interface/rosdoc.yaml: -------------------------------------------------------------------------------- 1 | - builder: epydoc 2 | name: Intera SDK Python API 3 | config: epydoc.config 4 | -------------------------------------------------------------------------------- /intera_interface/scripts/calibrate_arm.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | # Copyright (c) 2013-2018, Rethink Robotics Inc. 3 | # 4 | # Licensed under the Apache License, Version 2.0 (the "License"); 5 | # you may not use this file except in compliance with the License. 6 | # You may obtain a copy of the License at 7 | # 8 | # http://www.apache.org/licenses/LICENSE-2.0 9 | # 10 | # Unless required by applicable law or agreed to in writing, software 11 | # distributed under the License is distributed on an "AS IS" BASIS, 12 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | # See the License for the specific language governing permissions and 14 | # limitations under the License. 15 | 16 | 17 | import argparse 18 | import sys 19 | 20 | import rospy 21 | import actionlib 22 | 23 | import intera_interface 24 | from intera_interface import CHECK_VERSION 25 | from intera_core_msgs.msg import ( 26 | CalibrationCommandAction, 27 | CalibrationCommandGoal, 28 | ) 29 | 30 | class CalibrateArm(object): 31 | def __init__(self, limb="right"): 32 | """ 33 | @param limb: Limb to run CalibrateArm on arm side. 34 | """ 35 | self._limb=limb 36 | self._client = actionlib.SimpleActionClient('/calibration_command', 37 | CalibrationCommandAction) 38 | # Waits until the action server has started up and started 39 | # listening for goals. 40 | server_up = self._client.wait_for_server(rospy.Duration(10.0)) 41 | if not server_up: 42 | rospy.logerr("Timed out waiting for Calibration" 43 | " Server to connect. Check your ROS networking" 44 | " and/or reboot the robot.") 45 | rospy.signal_shutdown("Timed out waiting for Action Server") 46 | sys.exit(1) 47 | 48 | def _feedback(self, data): 49 | """ 50 | Prints Calibration Command Progress 51 | """ 52 | ratio=float(data.currentPoseNumber)/float(data.numberOfPoses) 53 | print('[{0}] {1}% complete - {2}'.format( 54 | ('#'*int(ratio*40)).ljust(40), 55 | '{0:.2f}'.format(ratio*100.0).rjust(5, ' '), 56 | data.currentState.ljust(10)), 57 | end='\r') 58 | 59 | def _send_calibration_msg(self, cmd): 60 | """ 61 | Sends Calibration Command request 62 | """ 63 | # Creates a goal to send to the action server. 64 | msg = CalibrationCommandGoal() 65 | msg.command = cmd 66 | # Sends the goal to the action server. 67 | rospy.loginfo("Sending Calibration Request {0}.".format( 68 | cmd.capitalize())) 69 | self._client.send_goal(msg, feedback_cb=self._feedback) 70 | # Waits for the server to finish performing the action. 71 | self._client.wait_for_result() 72 | # Prints out the result of executing the action 73 | return self._client.get_result() 74 | 75 | def stop_calibration(self): 76 | """ 77 | Preempts calibration execution by sending cancel goal 78 | """ 79 | return self._send_calibration_msg(CalibrationCommandGoal.CALIBRATION_STOP) 80 | 81 | def start_calibration(self): 82 | """ 83 | Starts calibration execution by sending goal 84 | """ 85 | rs = intera_interface.RobotEnable(CHECK_VERSION) 86 | # Enable robot 87 | rs.enable() 88 | l = intera_interface.Limb(self._limb) 89 | # Move slowly to set neutral pose 90 | rospy.loginfo("Moving {0} arm to neutral pose...".format(self._limb)) 91 | l.move_to_neutral(timeout=25.0, speed=0.1) 92 | # Ask robot to calibrate 93 | cal_result = self._send_calibration_msg(CalibrationCommandGoal.CALIBRATION_START) 94 | l.set_joint_position_speed(speed=0.3) 95 | return cal_result 96 | 97 | def is_gripper_removed(): 98 | """ 99 | Verify grippers are removed for calibration. 100 | """ 101 | try: 102 | gripper = intera_interface.get_current_gripper_interface() 103 | except Exception as e: 104 | return True 105 | rospy.logerr("Calibration Client: Cannot calibrate with grippers attached." 106 | " Remove grippers before calibration!") 107 | return False 108 | 109 | 110 | def main(): 111 | parser = argparse.ArgumentParser() 112 | parser.add_argument('-l', '--limb', 113 | choices=['left', 'right'], default="right", 114 | help="Calibrate the specified arm") 115 | args = parser.parse_args(rospy.myargv()[1:]) 116 | arm = args.limb 117 | 118 | print("Initializing node...") 119 | rospy.init_node('sdk_calibrate_arm_{0}'.format(arm), disable_signals=True) 120 | 121 | rospy.loginfo("Preparing to Calibrate...") 122 | gripper_warn = ("IMPORTANT: Make sure to remove grippers and other" 123 | " attachments before running Calibrate.") 124 | rospy.loginfo(gripper_warn) 125 | if not is_gripper_removed(): 126 | return 1 127 | 128 | ca = CalibrateArm(arm) 129 | 130 | error = None 131 | goal_state = "unreported error" 132 | rospy.loginfo("Running Calibrate on {0} arm".format(arm)) 133 | try: 134 | goal_state = ca.start_calibration() 135 | except KeyboardInterrupt as e: 136 | error = e 137 | goal_state = ca.stop_calibration() 138 | 139 | if error == None and "success" in str(goal_state).lower(): 140 | rospy.loginfo("Calibrate arm finished successfully. Please reboot your robot to use this calibration data.") 141 | else: 142 | error = True 143 | rospy.logerr("Calibrate arm failed with {0}".format(goal_state)) 144 | rospy.logerr("Please re-run this Calibration request.") 145 | 146 | return 0 if error == None else 1 147 | 148 | if __name__ == '__main__': 149 | sys.exit(main()) 150 | -------------------------------------------------------------------------------- /intera_interface/scripts/enable_robot.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | 3 | # Copyright (c) 2013-2018, Rethink Robotics Inc. 4 | # 5 | # Licensed under the Apache License, Version 2.0 (the "License"); 6 | # you may not use this file except in compliance with the License. 7 | # You may obtain a copy of the License at 8 | # 9 | # http://www.apache.org/licenses/LICENSE-2.0 10 | # 11 | # Unless required by applicable law or agreed to in writing, software 12 | # distributed under the License is distributed on an "AS IS" BASIS, 13 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | # See the License for the specific language governing permissions and 15 | # limitations under the License. 16 | 17 | import argparse 18 | import os 19 | import sys 20 | 21 | import rospy 22 | 23 | import intera_interface 24 | 25 | def main(): 26 | parser = argparse.ArgumentParser() 27 | parser.add_argument('-s', '--state', const='state', 28 | dest='actions', action='append_const', 29 | help='Print current robot state') 30 | parser.add_argument('-e', '--enable', const='enable', 31 | dest='actions', action='append_const', 32 | help='Enable the robot') 33 | parser.add_argument('-d', '--disable', const='disable', 34 | dest='actions', action='append_const', 35 | help='Disable the robot') 36 | parser.add_argument('-r', '--reset', const='reset', 37 | dest='actions', action='append_const', 38 | help='Reset the robot') 39 | parser.add_argument('-S', '--stop', const='stop', 40 | dest='actions', action='append_const', 41 | help='Stop the robot') 42 | args = parser.parse_args(rospy.myargv()[1:]) 43 | 44 | if args.actions == None: 45 | parser.print_usage() 46 | parser.exit(0, "No action defined") 47 | 48 | rospy.init_node('sdk_robot_enable') 49 | rs = intera_interface.RobotEnable(intera_interface.CHECK_VERSION) 50 | 51 | try: 52 | for act in args.actions: 53 | if act == 'state': 54 | print(rs.state()) 55 | elif act == 'enable': 56 | rs.enable() 57 | elif act == 'disable': 58 | rs.disable() 59 | elif act == 'reset': 60 | rs.reset() 61 | elif act == 'stop': 62 | rs.stop() 63 | except Exception as e: 64 | rospy.logerr(e.strerror) 65 | 66 | return 0 67 | 68 | if __name__ == '__main__': 69 | sys.exit(main()) 70 | -------------------------------------------------------------------------------- /intera_interface/scripts/home_joints.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | # Copyright (c) 2013-2018, Rethink Robotics Inc. 3 | # 4 | # Licensed under the Apache License, Version 2.0 (the "License"); 5 | # you may not use this file except in compliance with the License. 6 | # You may obtain a copy of the License at 7 | # 8 | # http://www.apache.org/licenses/LICENSE-2.0 9 | # 10 | # Unless required by applicable law or agreed to in writing, software 11 | # distributed under the License is distributed on an "AS IS" BASIS, 12 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | # See the License for the specific language governing permissions and 14 | # limitations under the License. 15 | 16 | import argparse 17 | import copy 18 | import threading 19 | 20 | import rospy 21 | import intera_interface 22 | from intera_interface import CHECK_VERSION 23 | from intera_core_msgs.msg import RobotAssemblyState, HomingState, HomingCommand 24 | 25 | class HomeJoints(object): 26 | def __init__(self): 27 | """ 28 | HomeJoints - Class that publishes on /robot/set_homing_mode to 29 | home the robot if it is not already homed. 30 | """ 31 | self._hcb_lock = threading.Lock() 32 | self._ecb_lock = threading.Lock() 33 | self._homing_state = dict() 34 | self._enable_state = False 35 | self._pub_home_joints = rospy.Publisher( 36 | '/robot/set_homing_mode', 37 | HomingCommand, 38 | latch=True, 39 | queue_size=10) 40 | self._enable_sub = rospy.Subscriber( 41 | '/robot/state', 42 | RobotAssemblyState, 43 | self._enable_state_cb) 44 | self._homing_sub = rospy.Subscriber( 45 | '/robot/homing_states', 46 | HomingState, 47 | self._homing_state_cb) 48 | 49 | def _enable_state_cb(self, msg): 50 | with self._ecb_lock: 51 | self._enable_state = msg.enabled 52 | 53 | def _robot_is_disabled(self): 54 | with self._ecb_lock: 55 | return False if self._enable_state else True 56 | 57 | def _homing_state_cb(self, msg): 58 | with self._hcb_lock: 59 | self._homing_state = dict(list(zip(msg.name, msg.state))) 60 | 61 | def _joints_are_homed(self): 62 | with self._hcb_lock: 63 | if not self._homing_state: 64 | return False 65 | for joint_name in self._homing_state: 66 | if self._homing_state[joint_name] != HomingState.HOMED: 67 | return False 68 | return True 69 | 70 | def home_robot(self, mode=HomingCommand.AUTO, timeout=60.0): 71 | sleep_rate = rospy.Rate(1) 72 | while not rospy.is_shutdown() and self._robot_is_disabled(): 73 | sleep_rate.sleep() 74 | start_time = rospy.Time.now() 75 | timeout_reached = lambda:((rospy.Time.now() - start_time).to_sec() > timeout) 76 | # Wait for First Homing State Callback 77 | homing_joints = list() 78 | while not rospy.is_shutdown(): 79 | if timeout_reached(): 80 | return False 81 | with self._hcb_lock: 82 | if bool(self._homing_state): 83 | homing_joints = copy.deepcopy(list(self._homing_state.keys())) 84 | break 85 | sleep_rate.sleep() 86 | # Construct Homing Command 87 | cmd = HomingCommand() 88 | cmd.name = homing_joints 89 | cmd.command = [mode]*len(homing_joints) 90 | # Publish & Wait for Joints to Home 91 | while not rospy.is_shutdown(): 92 | if timeout_reached(): 93 | break 94 | if not self._joints_are_homed(): 95 | # Publish Homing command 96 | self._pub_home_joints.publish(cmd) 97 | else: 98 | return True 99 | sleep_rate.sleep() 100 | return False 101 | 102 | def main(): 103 | parser = argparse.ArgumentParser() 104 | parser.add_argument("-t", "--timeout", type=lambda t:abs(float(t)), 105 | default=60.0, help="[in seconds] Time to wait for joints to home") 106 | parser.add_argument("-m", "--mode", type=str.upper, default="AUTO", 107 | choices=['AUTO', 'MANUAL'], help="Mode to home the robot's joints") 108 | enable_parser = parser.add_mutually_exclusive_group(required=False) 109 | enable_parser.add_argument("-e", "--enable", action='store_true', dest='enable', 110 | help="Try to enable the robot before homing.") 111 | enable_parser.add_argument("-n", "--no-enable", action='store_false', dest='enable', 112 | help="Avoid trying to enable the robot before homing.") 113 | enable_parser.set_defaults(enable=True) 114 | args = parser.parse_args(rospy.myargv()[1:]) 115 | 116 | rospy.init_node('home_joints_node') 117 | if args.enable: 118 | rs = intera_interface.RobotEnable(CHECK_VERSION) 119 | rs.enable() 120 | cmd_mode = HomingCommand.MANUAL if args.mode == 'MANUAL' else HomingCommand.AUTO 121 | rospy.loginfo("Homing joints in '{0}' mode".format(args.mode.capitalize())) 122 | home_jnts = HomeJoints() 123 | state = home_jnts.home_robot(mode=cmd_mode, timeout=args.timeout) 124 | rospy.loginfo("{0} in homing the robot's joints".format("Succeeded" if state else "Failed")) 125 | 126 | if __name__ == '__main__': 127 | main() 128 | -------------------------------------------------------------------------------- /intera_interface/scripts/io_config_editor.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | # Copyright (c) 2013-2018, Rethink Robotics Inc. 3 | # 4 | # Licensed under the Apache License, Version 2.0 (the "License"); 5 | # you may not use this file except in compliance with the License. 6 | # You may obtain a copy of the License at 7 | # 8 | # http://www.apache.org/licenses/LICENSE-2.0 9 | # 10 | # Unless required by applicable law or agreed to in writing, software 11 | # distributed under the License is distributed on an "AS IS" BASIS, 12 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | # See the License for the specific language governing permissions and 14 | # limitations under the License. 15 | 16 | """ 17 | SDK Read / Write ClickSmart (EndEffector) Configuration File 18 | """ 19 | 20 | import argparse 21 | import json 22 | 23 | import rospy 24 | 25 | import intera_interface 26 | 27 | 28 | def save_config(device, filename): 29 | """ 30 | Saves the current End Effector configuration on a ClickSmart device to the 31 | given local file as JSON. 32 | 33 | @type device: SimpleClickSmartGripper 34 | @param device: instance of ClickSmart for current gripper 35 | @type filename: str 36 | @param filename: path to output file where config will be written 37 | (will overwrite file if already exists) 38 | """ 39 | # get config from ClickSmart device 40 | try: 41 | config_msg = device.config 42 | except: 43 | # try epg structure 44 | config_msg = device.gripper_io.config 45 | device_name = config_msg.device.name 46 | config = config_msg.device.config 47 | 48 | # save config to file 49 | if filename: 50 | # parse string so output can be nicely formatted 51 | json_config = json.loads(config) 52 | with open(filename, 'w') as f: 53 | json.dump(json_config, f, ensure_ascii=False, sort_keys=True, 54 | indent=2, separators=(",", ": ")) 55 | else: 56 | rospy.logerr("Bad filename: '{}'".format(filename)) 57 | 58 | def load_config(device, filename): 59 | """ 60 | Read a JSON End Effector configuration from a local file and load it onto 61 | a ClickSmart attached to the robot. Reconfigures the ClickSmart and 62 | stores the configuration on the ClickSmart plate. 63 | 64 | @type device: SimpleClickSmartGripper 65 | @param device: instance of ClickSmart for current gripper 66 | @type filename: str 67 | @param filename: path to input file containing the JSON EE config 68 | """ 69 | with open(filename, 'r') as f: 70 | config_data = json.load(f) 71 | 72 | device.send_configuration(config_data) 73 | 74 | 75 | def main(): 76 | """Save / Load EndEffector Config utility 77 | 78 | Read current config off of ClickSmart and save to file. 79 | Or load config from file and reconfigure and save it to ClickSmart device. 80 | """ 81 | arg_fmt = argparse.RawDescriptionHelpFormatter 82 | parser = argparse.ArgumentParser(formatter_class=arg_fmt, 83 | description=main.__doc__) 84 | 85 | parser.add_argument( 86 | '-s', '--save', metavar='PATH', 87 | help='save current EE config to given file' 88 | ) 89 | parser.add_argument( 90 | '-l', '--load', metavar='PATH', 91 | help='load config from given file onto EE' 92 | ) 93 | args = parser.parse_args(rospy.myargv()[1:]) 94 | 95 | print("Initializing node... ") 96 | rospy.init_node('ee_config_editor', anonymous=True) 97 | 98 | ee = intera_interface.get_current_gripper_interface() 99 | if not ee: 100 | rospy.logerr("Could not detect an attached EndEffector!") 101 | return 102 | 103 | if args.save: 104 | rospy.loginfo("Saving EE config to {}".format(args.save)) 105 | save_config(ee, args.save) 106 | 107 | if args.load: 108 | rospy.loginfo("Loading config and writing config to ClickSmart from {}".format(args.load)) 109 | load_config(ee, args.load) 110 | 111 | def clean_shutdown(): 112 | print("\nExiting example...") 113 | 114 | rospy.on_shutdown(clean_shutdown) 115 | 116 | 117 | if __name__ == '__main__': 118 | main() 119 | -------------------------------------------------------------------------------- /intera_interface/scripts/joint_trajectory_action_server.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | # Copyright (c) 2013-2018, Rethink Robotics Inc. 3 | # 4 | # Licensed under the Apache License, Version 2.0 (the "License"); 5 | # you may not use this file except in compliance with the License. 6 | # You may obtain a copy of the License at 7 | # 8 | # http://www.apache.org/licenses/LICENSE-2.0 9 | # 10 | # Unless required by applicable law or agreed to in writing, software 11 | # distributed under the License is distributed on an "AS IS" BASIS, 12 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | # See the License for the specific language governing permissions and 14 | # limitations under the License. 15 | 16 | """ 17 | Intera SDK Joint Trajectory Controller 18 | Unlike other robots running ROS, this is not a Motor Controller plugin, 19 | but a regular node using the SDK interface. 20 | """ 21 | import argparse 22 | import importlib 23 | 24 | import rospy 25 | from dynamic_reconfigure.server import Server 26 | import intera_interface 27 | 28 | from intera_joint_trajectory_action.joint_trajectory_action import ( 29 | JointTrajectoryActionServer, 30 | ) 31 | from trajectory_msgs.msg import ( 32 | JointTrajectoryPoint, 33 | ) 34 | 35 | def start_server(limb, rate, mode, valid_limbs): 36 | rospy.loginfo("Initializing node... ") 37 | rospy.init_node("sdk_{0}_joint_trajectory_action_server{1}".format( 38 | mode, "" if limb == 'all_limbs' else "_" + limb,)) 39 | 40 | rospy.loginfo("Initializing joint trajectory action server...") 41 | robot_name = intera_interface.RobotParams().get_robot_name().lower().capitalize() 42 | config_module = "intera_interface.cfg" 43 | if mode == 'velocity': 44 | config_name = ''.join([robot_name,"VelocityJointTrajectoryActionServerConfig"]) 45 | elif mode == 'position': 46 | config_name = ''.join([robot_name,"PositionJointTrajectoryActionServerConfig"]) 47 | else: 48 | config_name = ''.join([robot_name,"PositionFFJointTrajectoryActionServerConfig"]) 49 | cfg = importlib.import_module('.'.join([config_module,config_name])) 50 | dyn_cfg_srv = Server(cfg, lambda config, level: config) 51 | jtas = [] 52 | if limb == 'all_limbs': 53 | for current_limb in valid_limbs: 54 | jtas.append(JointTrajectoryActionServer(current_limb, dyn_cfg_srv, 55 | rate, mode)) 56 | else: 57 | jtas.append(JointTrajectoryActionServer(limb, dyn_cfg_srv, rate, mode)) 58 | 59 | 60 | def cleanup(): 61 | for j in jtas: 62 | j.clean_shutdown() 63 | 64 | rospy.on_shutdown(cleanup) 65 | rospy.loginfo("Joint Trajectory Action Server Running. Ctrl-c to quit") 66 | rospy.spin() 67 | 68 | 69 | def main(): 70 | rp = intera_interface.RobotParams() 71 | valid_limbs = rp.get_limb_names() 72 | if not valid_limbs: 73 | rp.log_message(("Cannot detect any limb parameters on this robot. " 74 | "Exiting."), "ERROR") 75 | return 76 | # Add an option for starting a server for all valid limbs 77 | all_limbs = valid_limbs 78 | all_limbs.append("all_limbs") 79 | arg_fmt = argparse.ArgumentDefaultsHelpFormatter 80 | parser = argparse.ArgumentParser(formatter_class=arg_fmt) 81 | parser.add_argument( 82 | "-l", "--limb", dest="limb", default=valid_limbs[0], 83 | choices=all_limbs, 84 | help="joint trajectory action server limb" 85 | ) 86 | parser.add_argument( 87 | "-r", "--rate", dest="rate", default=100.0, 88 | type=float, help="trajectory control rate (Hz)" 89 | ) 90 | parser.add_argument( 91 | "-m", "--mode", default='position_w_id', 92 | choices=['position_w_id', 'position', 'velocity'], 93 | help="control mode for trajectory execution" 94 | ) 95 | args = parser.parse_args(rospy.myargv()[1:]) 96 | start_server(args.limb, args.rate, args.mode, valid_limbs) 97 | 98 | 99 | if __name__ == "__main__": 100 | main() 101 | -------------------------------------------------------------------------------- /intera_interface/scripts/send_urdf_fragment.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | 3 | # Copyright (c) 2013-2018, Rethink Robotics Inc. 4 | # 5 | # Licensed under the Apache License, Version 2.0 (the "License"); 6 | # you may not use this file except in compliance with the License. 7 | # You may obtain a copy of the License at 8 | # 9 | # http://www.apache.org/licenses/LICENSE-2.0 10 | # 11 | # Unless required by applicable law or agreed to in writing, software 12 | # distributed under the License is distributed on an "AS IS" BASIS, 13 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | # See the License for the specific language governing permissions and 15 | # limitations under the License. 16 | 17 | import os 18 | import sys 19 | import argparse 20 | 21 | import rospy 22 | try: 23 | import xacro_jade as xacro 24 | except ImportError: 25 | import xacro 26 | 27 | 28 | from intera_core_msgs.msg import ( 29 | URDFConfiguration, 30 | ) 31 | 32 | def xacro_parse(filename): 33 | doc = xacro.parse(None, filename) 34 | xacro.process_doc(doc, in_order=True) 35 | return doc.toprettyxml(indent=' ') 36 | 37 | def send_urdf(parent_link, root_joint, urdf_filename, duration): 38 | """ 39 | Send the URDF Fragment located at the specified path. 40 | 41 | @param parent_link: parent link to attach the URDF fragment to 42 | (usually _hand) 43 | @param root_joint: root link of the URDF fragment (usually _gripper_base) 44 | @param urdf_filename: path to the urdf XML file to load into xacro and send 45 | @param duration: duration to repeat sending the URDF to ensure it is received 46 | """ 47 | msg = URDFConfiguration() 48 | # The updating the time parameter tells 49 | # the robot that this is a new configuration. 50 | # Only update the time when an updated internal 51 | # model is required. Do not continuously update 52 | # the time parameter. 53 | msg.time = rospy.Time.now() 54 | # link to attach this urdf to onboard the robot 55 | msg.link = parent_link 56 | # root linkage in your URDF Fragment 57 | msg.joint = root_joint 58 | msg.urdf = xacro_parse(urdf_filename) 59 | pub = rospy.Publisher('/robot/urdf', URDFConfiguration, queue_size=10) 60 | rate = rospy.Rate(5) # 5hz 61 | start = rospy.Time.now() 62 | while not rospy.is_shutdown(): 63 | pub.publish(msg) 64 | rate.sleep() 65 | if (rospy.Time.now() - msg.time) > rospy.Duration(duration): 66 | break 67 | 68 | def main(): 69 | """RSDK URDF Fragment Example: 70 | This example shows a proof of concept for 71 | adding your URDF fragment to the robot's 72 | onboard URDF (which is currently in use). 73 | """ 74 | arg_fmt = argparse.RawDescriptionHelpFormatter 75 | parser = argparse.ArgumentParser(formatter_class=arg_fmt, 76 | description=main.__doc__) 77 | required = parser.add_argument_group('required arguments') 78 | required.add_argument( 79 | '-f', '--file', metavar='PATH', required=True, 80 | help='Path to URDF file to send' 81 | ) 82 | required.add_argument( 83 | '-l', '--link', required=False, default="right_hand", 84 | help='URDF Link already to attach fragment to (usually _hand)' 85 | ) 86 | required.add_argument( 87 | '-j', '--joint', required=False, default="right_gripper_base", 88 | help='Root joint for fragment (usually _gripper_base)' 89 | ) 90 | parser.add_argument("-d", "--duration", type=lambda t:abs(float(t)), 91 | default=5.0, help="[in seconds] Duration to publish fragment") 92 | args = parser.parse_args(rospy.myargv()[1:]) 93 | 94 | rospy.init_node('rsdk_configure_urdf', anonymous=True) 95 | 96 | if not os.access(args.file, os.R_OK): 97 | rospy.logerr("Cannot read file at '%s'" % (args.file,)) 98 | return 1 99 | send_urdf(args.link, args.joint, args.file, args.duration) 100 | return 0 101 | 102 | if __name__ == '__main__': 103 | sys.exit(main()) 104 | -------------------------------------------------------------------------------- /intera_interface/setup.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | from setuptools import setup 3 | from catkin_pkg.python_setup import generate_distutils_setup 4 | 5 | d = generate_distutils_setup() 6 | d['packages'] = ['intera_interface', 'intera_control', 'intera_dataflow', 7 | 'intera_io', 'intera_joint_trajectory_action', 'intera_motion_interface'] 8 | d['package_dir'] = {'': 'src'} 9 | 10 | setup(**d) 11 | -------------------------------------------------------------------------------- /intera_interface/src/intera_control/__init__.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2013-2018, Rethink Robotics Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from .pid import PID 16 | -------------------------------------------------------------------------------- /intera_interface/src/intera_control/pid.py: -------------------------------------------------------------------------------- 1 | # Modified example source code from the book 2 | # "Real-World Instrumentation with Python" 3 | # by J. M. Hughes, published by O'Reilly Media, December 2010, 4 | # ISBN 978-0-596-80956-0. 5 | 6 | import rospy 7 | 8 | 9 | class PID(object): 10 | """ 11 | PID control class 12 | 13 | This class implements a simplistic PID control algorithm. When first 14 | instantiated all the gain variables are set to zero, so calling 15 | the method compute_output will just return zero. 16 | """ 17 | 18 | def __init__(self, kp=0.0, ki=0.0, kd=0.0): 19 | # initialize gains 20 | self._kp = kp 21 | self._ki = ki 22 | self._kd = kd 23 | 24 | # initialize error, results, and time descriptors 25 | self._prev_err = 0.0 26 | self._cp = 0.0 27 | self._ci = 0.0 28 | self._cd = 0.0 29 | self._cur_time = 0.0 30 | self._prev_time = 0.0 31 | 32 | self.initialize() 33 | 34 | def initialize(self): 35 | """ 36 | Initialize pid controller. 37 | """ 38 | # reset delta t variables 39 | self._cur_time = rospy.get_time() 40 | self._prev_time = self._cur_time 41 | 42 | self._prev_err = 0.0 43 | 44 | # reset result variables 45 | self._cp = 0.0 46 | self._ci = 0.0 47 | self._cd = 0.0 48 | 49 | def set_kp(self, invar): 50 | """ 51 | Set proportional gain. 52 | """ 53 | self._kp = invar 54 | 55 | def set_ki(self, invar): 56 | """ 57 | Set integral gain. 58 | """ 59 | self._ki = invar 60 | 61 | def set_kd(self, invar): 62 | """ 63 | Set derivative gain. 64 | """ 65 | self._kd = invar 66 | 67 | def compute_output(self, error): 68 | """ 69 | Performs a PID computation and returns a control value based on 70 | the elapsed time (dt) and the error signal from a summing junction 71 | (the error parameter). 72 | """ 73 | self._cur_time = rospy.get_time() # get t 74 | dt = self._cur_time - self._prev_time # get delta t 75 | de = error - self._prev_err # get delta error 76 | 77 | self._cp = error # proportional term 78 | self._ci += error * dt # integral term 79 | 80 | self._cd = 0 81 | if dt > 0: # no div by zero 82 | self._cd = de / dt # derivative term 83 | 84 | self._prev_time = self._cur_time # save t for next pass 85 | self._prev_err = error # save t-1 error 86 | 87 | # sum the terms and return the result 88 | return ((self._kp * self._cp) + (self._ki * self._ci) + 89 | (self._kd * self._cd)) 90 | -------------------------------------------------------------------------------- /intera_interface/src/intera_dataflow/__init__.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2013-2018, Rethink Robotics Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from .wait_for import wait_for 16 | from .signals import Signal 17 | -------------------------------------------------------------------------------- /intera_interface/src/intera_dataflow/signals.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2013-2018, Rethink Robotics Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | import inspect 16 | from weakref import WeakKeyDictionary 17 | 18 | try: 19 | from weakref import WeakSet 20 | except ImportError: 21 | from .weakrefset import WeakSet 22 | 23 | 24 | class Signal(object): 25 | def __init__(self): 26 | self._functions = WeakSet() 27 | self._methods = WeakKeyDictionary() 28 | 29 | def __call__(self, *args, **kargs): 30 | for f in self._functions: 31 | f(*args, **kargs) 32 | 33 | for obj, functions in list(self._methods.items()): 34 | for f in functions: 35 | f(obj, *args, **kargs) 36 | 37 | def connect(self, slot): 38 | if inspect.ismethod(slot): 39 | if not slot.__self__ in self._methods: 40 | self._methods[slot.__self__] = set() 41 | self._methods[slot.__self__].add(slot.__func__) 42 | else: 43 | self._functions.add(slot) 44 | 45 | def disconnect(self, slot): 46 | if inspect.ismethod(slot): 47 | if slot.__self__ in self._methods: 48 | self._methods[slot.__self__].remove(slot.__func__) 49 | else: 50 | if slot in self._functions: 51 | self._functions.remove(slot) 52 | -------------------------------------------------------------------------------- /intera_interface/src/intera_dataflow/wait_for.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2013-2018, Rethink Robotics Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | import errno 16 | 17 | import rospy 18 | 19 | 20 | def wait_for(test, timeout=10.0, raise_on_error=True, rate=100, 21 | timeout_msg="timeout expired", body=None): 22 | """ 23 | Waits until some condition evaluates to true. 24 | 25 | @param test: zero param function to be evaluated 26 | @param timeout: max amount of time to wait. negative/inf for indefinitely 27 | @param raise_on_error: raise or just return False 28 | @param rate: the rate at which to check 29 | @param timout_msg: message to supply to the timeout exception 30 | @param body: optional function to execute while waiting 31 | """ 32 | end_time = rospy.get_time() + timeout 33 | rate = rospy.Rate(rate) 34 | notimeout = (timeout < 0.0) or timeout == float("inf") 35 | while not test(): 36 | if rospy.is_shutdown(): 37 | if raise_on_error: 38 | raise OSError(errno.ESHUTDOWN, "ROS Shutdown") 39 | return False 40 | elif (not notimeout) and (rospy.get_time() >= end_time): 41 | if raise_on_error: 42 | raise OSError(errno.ETIMEDOUT, timeout_msg) 43 | return False 44 | if callable(body): 45 | body() 46 | rate.sleep() 47 | return True 48 | -------------------------------------------------------------------------------- /intera_interface/src/intera_interface/__init__.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2013-2018, Rethink Robotics Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from .camera import Cameras 16 | from .digital_io import DigitalIO 17 | from .gripper import Gripper 18 | from .clicksmart_plate import SimpleClickSmartGripper 19 | from .gripper_factory import get_current_gripper_interface 20 | from .cuff import Cuff 21 | from .head import Head 22 | from .head_display import HeadDisplay 23 | from .joint_limits import JointLimits 24 | from .lights import Lights 25 | from .limb import Limb 26 | from .navigator import Navigator 27 | from .robot_enable import RobotEnable 28 | from .robot_params import RobotParams 29 | from .settings import ( 30 | JOINT_ANGLE_TOLERANCE, 31 | HEAD_PAN_ANGLE_TOLERANCE, 32 | SDK_VERSION, 33 | CHECK_VERSION, 34 | VERSIONS_SDK2ROBOT, 35 | VERSIONS_SDK2GRIPPER, 36 | ) 37 | -------------------------------------------------------------------------------- /intera_interface/src/intera_interface/cuff.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2013-2018, Rethink Robotics Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | import rospy 16 | import intera_dataflow 17 | from intera_io import IODeviceInterface 18 | from intera_core_msgs.msg import IODeviceConfiguration 19 | from .robot_params import RobotParams 20 | 21 | 22 | class Cuff(object): 23 | """ 24 | Interface class for cuff on the Intera robots. 25 | """ 26 | 27 | def __init__(self, limb="right"): 28 | """ 29 | Constructor. 30 | 31 | @type limb: str 32 | @param limb: limb side to interface 33 | """ 34 | params = RobotParams() 35 | limb_names = params.get_limb_names() 36 | if limb not in limb_names: 37 | rospy.logerr("Cannot detect Cuff's limb {0} on this robot." 38 | " Valid limbs are {1}. Exiting Cuff.init().".format( 39 | limb, limb_names)) 40 | return 41 | self.limb = limb 42 | self.device = None 43 | self.name = "cuff" 44 | self.cuff_config_sub = rospy.Subscriber('/io/robot/cuff/config', IODeviceConfiguration, self._config_callback) 45 | # Wait for the cuff status to be true 46 | intera_dataflow.wait_for( 47 | lambda: not self.device is None, timeout=5.0, 48 | timeout_msg=("Failed find cuff on limb '{0}'.".format(limb)) 49 | ) 50 | self._cuff_io = IODeviceInterface("robot", self.name) 51 | 52 | def _config_callback(self, msg): 53 | """ 54 | config topic callback 55 | """ 56 | if msg.device != []: 57 | if str(msg.device.name) == self.name: 58 | self.device = msg.device 59 | 60 | def lower_button(self): 61 | """ 62 | Returns a boolean describing whether the lower button on cuff is pressed. 63 | 64 | @rtype: bool 65 | @return: a variable representing button state: (True: pressed, False: unpressed) 66 | """ 67 | return bool(self._cuff_io.get_signal_value('_'.join([self.limb, "button_lower"]))) 68 | 69 | def upper_button(self): 70 | """ 71 | Returns a boolean describing whether the upper button on cuff is pressed. 72 | (True: pressed, False: unpressed) 73 | @rtype: bool 74 | @return: a variable representing button state: (True: pressed, False: unpressed) 75 | """ 76 | return bool(self._cuff_io.get_signal_value('_'.join([self.limb, "button_upper"]))) 77 | 78 | def cuff_button(self): 79 | """ 80 | Returns a boolean describing whether the cuff button on cuff is pressed. 81 | (True: pressed, False: unpressed) 82 | @rtype: bool 83 | @return: a variable representing cuff button state: (True: pressed, False: unpressed) 84 | """ 85 | return bool(self._cuff_io.get_signal_value('_'.join([self.limb, "cuff"]))) 86 | 87 | def register_callback(self, callback_function, signal_name, poll_rate=10): 88 | """ 89 | Registers a supplied callback to a change in state of supplied 90 | signal_name's value. Spawns a thread that will call the callback with 91 | the updated value. 92 | 93 | @type callback_function: function 94 | @param callback_function: function handle for callback function 95 | @type signal_name: str 96 | @param signal_name: the name of the signal to poll for value change 97 | @type poll_rate: int 98 | @param poll_rate: the rate at which to poll for a value change (in a separate 99 | thread) 100 | 101 | @rtype: str 102 | @return: callback_id retuned if the callback was registered, and an 103 | empty string if the requested signal_name does not exist in the 104 | Navigator 105 | """ 106 | return self._cuff_io.register_callback( 107 | callback_function=callback_function, 108 | signal_name=signal_name, 109 | poll_rate=poll_rate) 110 | 111 | def deregister_callback(self, callback_id): 112 | """ 113 | Deregisters a callback based on the supplied callback_id. 114 | 115 | @type callback_id: str 116 | @param callback_id: the callback_id string to deregister 117 | 118 | @rtype: bool 119 | @return: returns bool True if the callback was successfully 120 | deregistered, and False otherwise. 121 | """ 122 | return self._cuff_io.deregister_callback(callback_id) 123 | -------------------------------------------------------------------------------- /intera_interface/src/intera_interface/digital_io.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2013-2018, Rethink Robotics Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | import errno 16 | 17 | import rospy 18 | 19 | import intera_dataflow 20 | 21 | from intera_core_msgs.msg import ( 22 | DigitalIOState, 23 | DigitalOutputCommand, 24 | ) 25 | 26 | 27 | class DigitalIO(object): 28 | """ 29 | DEPRECATION WARNING: This interface will likely be removed in 30 | the future. Transition to using the IO Framework and the wrapper 31 | classes: gripper.py, cuff.py, camera.py 32 | 33 | Interface class for a simple Digital Input and/or Output on the 34 | Intera robots. 35 | 36 | Input 37 | - read input state 38 | Output 39 | - turn output On/Off 40 | - read current output state 41 | """ 42 | def __init__(self, component_id): 43 | """ 44 | Constructor. 45 | 46 | @param component_id: unique id of the digital component 47 | """ 48 | self._id = component_id 49 | self._component_type = 'digital_io' 50 | self._is_output = False 51 | self._state = None 52 | 53 | self.state_changed = intera_dataflow.Signal() 54 | 55 | type_ns = '/robot/' + self._component_type 56 | topic_base = type_ns + '/' + self._id 57 | 58 | self._sub_state = rospy.Subscriber( 59 | topic_base + '/state', 60 | DigitalIOState, 61 | self._on_io_state) 62 | 63 | intera_dataflow.wait_for( 64 | lambda: self._state != None, 65 | timeout=2.0, 66 | timeout_msg="Failed to get current digital_io state from %s" \ 67 | % (topic_base,), 68 | ) 69 | 70 | # check if output-capable before creating publisher 71 | if self._is_output: 72 | self._pub_output = rospy.Publisher( 73 | type_ns + '/command', 74 | DigitalOutputCommand, 75 | queue_size=10) 76 | 77 | def _on_io_state(self, msg): 78 | """ 79 | Updates the internally stored state of the Digital Input/Output. 80 | """ 81 | new_state = (msg.state == DigitalIOState.PRESSED) 82 | if self._state is None: 83 | self._is_output = not msg.isInputOnly 84 | old_state = self._state 85 | self._state = new_state 86 | 87 | # trigger signal if changed 88 | if old_state is not None and old_state != new_state: 89 | self.state_changed(new_state) 90 | 91 | @property 92 | def is_output(self): 93 | """ 94 | Accessor to check if IO is capable of output. 95 | """ 96 | return self._is_output 97 | 98 | @property 99 | def state(self): 100 | """ 101 | Current state of the Digital Input/Output. 102 | """ 103 | return self._state 104 | 105 | @state.setter 106 | def state(self, value): 107 | """ 108 | Control the state of the Digital Output. (is_output must be True) 109 | 110 | @type value: bool 111 | @param value: new state to output {True, False} 112 | """ 113 | self.set_output(value) 114 | 115 | def set_output(self, value, timeout=2.0): 116 | """ 117 | Control the state of the Digital Output. 118 | 119 | Use this function for finer control over the wait_for timeout. 120 | 121 | @type value: bool 122 | @param value: new state {True, False} of the Output. 123 | @type timeout: float 124 | @param timeout: Seconds to wait for the io to reflect command. 125 | If 0, just command once and return. [0] 126 | """ 127 | if not self._is_output: 128 | raise IOError(errno.EACCES, "Component is not an output [%s: %s]" % 129 | (self._component_type, self._id)) 130 | cmd = DigitalOutputCommand() 131 | cmd.name = self._id 132 | cmd.value = value 133 | self._pub_output.publish(cmd) 134 | 135 | if not timeout == 0: 136 | intera_dataflow.wait_for( 137 | test=lambda: self.state == value, 138 | timeout=timeout, 139 | rate=100, 140 | timeout_msg=("Failed to command digital io to: %r" % (value,)), 141 | body=lambda: self._pub_output.publish(cmd) 142 | ) 143 | -------------------------------------------------------------------------------- /intera_interface/src/intera_interface/gripper_factory.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2013-2018, Rethink Robotics Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | import json 16 | import rospy 17 | from intera_core_msgs.msg import ( 18 | IONodeConfiguration, 19 | IONodeStatus, 20 | IOComponentConfiguration 21 | ) 22 | import intera_dataflow 23 | from intera_interface import ( 24 | Gripper, 25 | SimpleClickSmartGripper 26 | ) 27 | 28 | 29 | def get_current_gripper_interface(): 30 | """ 31 | Instantiate and return an interface to control the gripper 32 | currently attached to the robot. 33 | 34 | Looks for the type of the first detected gripper attached to the robot 35 | and returns an instance of a Gripper or SimpleClickSmartGripper class 36 | configured to control the EE. 37 | 38 | @rtype: Gripper|SimpleClickSmartGripper 39 | @return: instance of an interface to control attached gripper 40 | """ 41 | gf = GripperFactory() 42 | return gf.get_current_gripper_interface() 43 | 44 | 45 | class GripperFactory(object): 46 | """ 47 | Helper class for creating a Gripper interface for the End Effector 48 | currently connected to the robot. Use the get_current_gripper_interface() fn. 49 | 50 | Listens to the end_effector IO Node for a list of End Effectors currently 51 | attached. The fn get_current_gripper_interface() will determine the type of the 52 | first currently connected gripper and return an instance of a corresponding 53 | SDK Gripper class to control the gripper. 54 | """ 55 | def __init__(self): 56 | self.states = [] 57 | self.configs = [] 58 | self._node_state = None 59 | self._node_config = None 60 | self._node_state_sub = rospy.Subscriber('/io/end_effector/state', IONodeStatus, self._node_state_cb) 61 | self._node_config_sub = rospy.Subscriber('/io/end_effector/config', IONodeConfiguration, self._node_config_cb) 62 | 63 | intera_dataflow.wait_for( 64 | lambda: self._node_state is not None, 65 | timeout=5.0, 66 | timeout_msg=("Failed to connect to end_effector IO Node.") 67 | ) 68 | 69 | def _node_state_cb(self, msg): 70 | self._node_state = msg 71 | if msg.devices: 72 | self.states = msg.devices 73 | 74 | def _node_config_cb(self, msg): 75 | self._node_config = msg 76 | if msg.devices: 77 | self.configs = msg.devices 78 | 79 | def _lookup_gripper_class(self, ee_type): 80 | EE_CLASS_MAP = dict({ 81 | "SmartToolPlate": SimpleClickSmartGripper, 82 | "ElectricParallelGripper": Gripper, 83 | "default": Gripper 84 | }) 85 | 86 | return EE_CLASS_MAP.get(ee_type, None) or EE_CLASS_MAP["default"] 87 | 88 | def _parse_config(self, config): 89 | ee_config = config 90 | if type(ee_config) == IOComponentConfiguration: 91 | ee_config = ee_config.config 92 | if type(ee_config) == str: 93 | ee_config = json.loads(ee_config) 94 | if type(ee_config) == dict: 95 | return ee_config 96 | 97 | def get_current_gripper_interface(self): 98 | """ 99 | Instantiate and return an interface to control the gripper 100 | currently attached to the robot. 101 | 102 | Looks for the type of the first detected gripper attached to the robot 103 | and returns an instance of a Gripper or SimpleClickSmartGripper class 104 | configured to control the EE. 105 | 106 | @rtype: Gripper|SimpleClickSmartGripper 107 | @return: instance of an interface to control attached gripper 108 | """ 109 | gripper = None 110 | 111 | if len(self.states) <= 0 or len(self.configs) <= 0: 112 | # wait a moment for any attached grippers to populate 113 | intera_dataflow.wait_for( 114 | lambda: (len(self.states) > 0 and len(self.configs) > 0), 115 | timeout=5.0, 116 | timeout_msg=("Failed to get gripper. No gripper attached on the robot.") 117 | ) 118 | 119 | ee_state = self.states[0] 120 | ee_id = ee_state.name 121 | ee_config = None 122 | for device in self.configs: 123 | if device.name == ee_id: 124 | ee_config = self._parse_config(device.config) 125 | break 126 | 127 | gripper_class = self._lookup_gripper_class(ee_config['props']['type']) 128 | needs_init = (ee_state.status.tag == 'down' or ee_state.status.tag == 'unready') 129 | try: 130 | gripper = gripper_class(ee_id, needs_init) 131 | except: 132 | gripper = gripper_class(ee_id, False) 133 | 134 | return gripper 135 | -------------------------------------------------------------------------------- /intera_interface/src/intera_interface/head_display.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2013-2018, Rethink Robotics Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | import os 16 | import cv2 17 | import cv_bridge 18 | import rospy 19 | 20 | from sensor_msgs.msg import Image 21 | 22 | 23 | class HeadDisplay(object): 24 | """ 25 | Interface class for head display 26 | 27 | Displays a given image file or multiple files on the robot's face. 28 | 29 | Pass the relative or absolute file path to an image file on your 30 | computer, and the example will read and convert the image using 31 | cv_bridge, sending it to the screen as a standard ROS Image Message. 32 | 33 | Notes: 34 | Max screen resolution is 1024x600. 35 | Images are always aligned to the top-left corner. 36 | Image formats are those supported by OpenCv - LoadImage(). 37 | """ 38 | 39 | def __init__(self): 40 | """ 41 | Constructor 42 | """ 43 | self._image_pub = rospy.Publisher('/robot/head_display', Image, latch=True, queue_size=10) 44 | 45 | def _setup_image(self, image_path): 46 | """ 47 | Load the image located at the specified path 48 | 49 | @type image_path: str 50 | @param image_path: the relative or absolute file path to the image file 51 | 52 | @rtype: sensor_msgs/Image or None 53 | @param: Returns sensor_msgs/Image if image convertable and None otherwise 54 | """ 55 | if not os.access(image_path, os.R_OK): 56 | rospy.logerr("Cannot read file at '{0}'".format(image_path)) 57 | return None 58 | 59 | img = cv2.imread(image_path) 60 | # Return msg 61 | return cv_bridge.CvBridge().cv2_to_imgmsg(img, encoding="bgr8") 62 | 63 | def display_image(self, image_path, display_in_loop=False, display_rate=1.0): 64 | """ 65 | Displays image(s) to robot's head 66 | 67 | @type image_path: list 68 | @param image_path: the relative or absolute file path to the image file(s) 69 | 70 | @type display_in_loop: bool 71 | @param display_in_loop: Set True to loop through all image files infinitely 72 | 73 | @type display_rate: float 74 | @param display_rate: the rate to publish image into head 75 | """ 76 | rospy.logdebug("Display images in loop:'{0}', frequency: '{1}'".format(display_in_loop, display_rate)) 77 | 78 | image_msg = [] 79 | image_list = image_path if isinstance(image_path, list) else [image_path] 80 | for one_path in image_list: 81 | cv_img = self._setup_image(one_path) 82 | if cv_img: 83 | image_msg.append(cv_img) 84 | 85 | if not image_msg: 86 | rospy.logerr("Image message list is empty") 87 | else: 88 | r = rospy.Rate(display_rate) 89 | while not rospy.is_shutdown(): 90 | for one_msg in image_msg: 91 | self._image_pub.publish(one_msg) 92 | r.sleep() 93 | if not display_in_loop: 94 | break 95 | -------------------------------------------------------------------------------- /intera_interface/src/intera_interface/lights.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2013-2018, Rethink Robotics Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | import rospy 16 | from intera_io import IODeviceInterface 17 | 18 | 19 | class Lights(object): 20 | """ 21 | Interface class for the lights on the Intera robots. 22 | """ 23 | 24 | def __init__(self): 25 | """ 26 | Constructor. 27 | 28 | """ 29 | self._lights_io = IODeviceInterface("robot", "robot") 30 | 31 | def list_all_lights(self): 32 | """ 33 | Returns a list of strings describing all available lights 34 | 35 | @rtype: list [str] 36 | @return: a list of string representing light names 37 | Each light name of the following format: 38 | '__light' 39 | """ 40 | return [name for name in self._lights_io.list_signal_names() if "light" in name] 41 | 42 | def set_light_state(self, name, on=True): 43 | """ 44 | Sets the named light the desired state (on=True, off=False) 45 | 46 | @type name: str 47 | @param name: Light name of the following format: 48 | '__light' 49 | @type on: bool 50 | @param on: value to set the light (on=True, off=False) 51 | 52 | @rtype: bool 53 | @return: True if the light state is set, False otherwise 54 | """ 55 | return self._lights_io.set_signal_value(name, on) 56 | 57 | def get_light_state(self, name): 58 | """ 59 | Returns a boolean describing whether the requested light is 'ON'. 60 | (True: on, False: off) 61 | 62 | @type name: str 63 | @param name: Light name of the following format: 64 | '__light' 65 | @rtype: bool 66 | @return: a variable representing light state: (True: on, False: off) 67 | """ 68 | return self._lights_io.get_signal_value(name) 69 | -------------------------------------------------------------------------------- /intera_interface/src/intera_interface/navigator.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2013-2018, Rethink Robotics Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | import threading 16 | import uuid 17 | 18 | import rospy 19 | 20 | import intera_dataflow 21 | from intera_io import IODeviceInterface 22 | 23 | class Navigator(object): 24 | """ 25 | Interface class for a Navigator on the Intera Research Robot. 26 | 27 | Signals: 28 | button_square_changed - OFF/CLICK/LONG_PRESS/DOUBLE_CLICK 29 | button_ok_changed 30 | button_back_changed 31 | button_show_changed 32 | button_triangle_changed 33 | button_circle_changed 34 | wheel_changed - Wheel value 35 | 36 | """ 37 | 38 | def __init__(self): 39 | """ 40 | Constructor. 41 | 42 | """ 43 | self._navigator_io = IODeviceInterface("robot", "navigator") 44 | self._button_lookup = {0:'OFF', 1:'CLICK', 45 | 2:'LONG_PRESS', 3:'DOUBLE_CLICK'} 46 | 47 | def list_all_items(self): 48 | """ 49 | Returns a list of strings describing all available navigator items 50 | 51 | @rtype: list 52 | @return: a list of string representing navigator items 53 | Each item name of the following format: 54 | '_button_' 55 | """ 56 | return self._navigator_io.list_signal_names() 57 | 58 | def get_wheel_state(self, wheel_name): 59 | """ 60 | Current state of the wheel providing wheel name 61 | @type wheel_name: str 62 | @param wheel_name: the wheel name 63 | 64 | @rtype: uint 65 | @return: an integer representing how far the wheel has turned 66 | """ 67 | return self._get_item_state(wheel_name) 68 | 69 | def get_button_state(self, button_name): 70 | """ 71 | Current button state by providing button name 72 | @type button_name: str 73 | @param button_name: the button name 74 | 75 | @rtype: uint 76 | @return: an integer representing button values 77 | Valid states: 78 | {0:'OFF', 1:'CLICK', 2:'LONG_PRESS', 3:'DOUBLE_CLICK'} 79 | """ 80 | return self._get_item_state(button_name) 81 | 82 | def button_string_lookup(self, button_value): 83 | """ 84 | Returns strings corresponding to the button state. 85 | 86 | @type button_value: int 87 | @param button_value: the value to lookup 88 | 89 | @rtype: str 90 | @return: 'INVALID_VALUE' if out of range, or if valid: 91 | {0:'OFF', 1:'CLICK', 2:'LONG_PRESS', 3:'DOUBLE_CLICK'} 92 | """ 93 | if button_value in self._button_lookup: 94 | return self._button_lookup[button_value] 95 | else: 96 | return 'INVALID_VALUE' 97 | 98 | def register_callback(self, callback_function, signal_name, poll_rate=10): 99 | """ 100 | Registers a supplied callback to a change in state of supplied 101 | signal_name's value. Spawns a thread that will call the callback with 102 | the updated value. 103 | 104 | @type callback_function: function 105 | @param callback_function: function handle for callback function 106 | @type signal_name: str 107 | @param signal_name: the name of the signal to poll for value change 108 | @type poll_rate: int 109 | @param poll_rate: the rate at which to poll for a value change (in a separate 110 | thread) 111 | 112 | @rtype: str 113 | @return: callback_id retuned if the callback was registered, and an 114 | empty string if the requested signal_name does not exist in the 115 | Navigator 116 | """ 117 | return self._navigator_io.register_callback( 118 | callback_function=callback_function, 119 | signal_name=signal_name, 120 | poll_rate=poll_rate) 121 | 122 | def deregister_callback(self, callback_id): 123 | """ 124 | Deregisters a callback based on the supplied callback_id. 125 | 126 | @type callback_id: str 127 | @param callback_id: the callback_id string to deregister 128 | 129 | @rtype: bool 130 | @return: returns bool True if the callback was successfully 131 | deregistered, and False otherwise. 132 | """ 133 | return self._navigator_io.deregister_callback(callback_id) 134 | 135 | def _get_item_state(self, item_name): 136 | return self._navigator_io.get_signal_value(item_name) 137 | 138 | -------------------------------------------------------------------------------- /intera_interface/src/intera_interface/robot_params.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2013-2018, Rethink Robotics Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | import rospy 16 | import socket 17 | 18 | class RobotParams(object): 19 | """ 20 | Interface class for essential ROS parameters on Intera robot. 21 | """ 22 | 23 | def __init__(self): 24 | self._sdk_networking_url = "http://sdk.rethinkrobotics.com/intera/Networking" 25 | self._ros_networking_url = "http://wiki.ros.org/ROS/NetworkSetup" 26 | self._color_dict = {"INFO":'37',"WARN":'33',"ERROR":'31'} 27 | self._color_prefix = "\033[1;{0}m" 28 | self._color_suffix = "\033[1;m" 29 | 30 | def get_camera_names(self): 31 | """ Return the names of the camera. 32 | @rtype: list [str] 33 | @return: ordered list of camera names 34 | """ 35 | return list(self.get_camera_details().keys()) 36 | 37 | def get_camera_details(self): 38 | """ 39 | Return the details of the cameras. 40 | 41 | @rtype: list [str] 42 | @return: ordered list of camera details 43 | """ 44 | camera_dict = dict() 45 | camera_config_param = "/robot_config/camera_config" 46 | try: 47 | camera_dict = rospy.get_param(camera_config_param) 48 | except KeyError: 49 | rospy.logerr("RobotParam:get_camera_details cannot detect any " 50 | "cameras under the parameter {0}".format(camera_config_param)) 51 | except (socket.error, socket.gaierror): 52 | self._log_networking_error() 53 | return camera_dict 54 | 55 | def get_limb_names(self): 56 | """ 57 | Return the names of the robot's articulated limbs from ROS parameter. 58 | 59 | @rtype: list [str] 60 | @return: ordered list of articulated limbs names 61 | (e.g. right, left). on networked robot 62 | """ 63 | non_limb_assemblies = ['torso', 'head'] 64 | return list(set(self.get_robot_assemblies()).difference(non_limb_assemblies)) 65 | 66 | def get_robot_assemblies(self): 67 | """ 68 | Return the names of the robot's assemblies from ROS parameter. 69 | 70 | @rtype: list [str] 71 | @return: ordered list of assembly names 72 | (e.g. right, left, torso, head). on networked robot 73 | """ 74 | assemblies = list() 75 | try: 76 | assemblies = rospy.get_param("/robot_config/assembly_names") 77 | except KeyError: 78 | rospy.logerr("RobotParam:get_robot_assemblies cannot detect assembly names" 79 | " under param /robot_config/assembly_names") 80 | except (socket.error, socket.gaierror): 81 | self._log_networking_error() 82 | return assemblies 83 | 84 | def get_joint_names(self, limb_name): 85 | """ 86 | Return the names of the joints for the specified 87 | limb from ROS parameter. 88 | 89 | @type limb_name: str 90 | @param limb_name: name of the limb for which to retrieve joint names 91 | 92 | @rtype: list [str] 93 | @return: ordered list of joint names from proximal to distal 94 | (i.e. shoulder to wrist). joint names for limb 95 | """ 96 | joint_names = list() 97 | try: 98 | joint_names = rospy.get_param( 99 | "robot_config/{0}_config/joint_names".format(limb_name)) 100 | except KeyError: 101 | rospy.logerr(("RobotParam:get_joint_names cannot detect joint_names for" 102 | " arm \"{0}\"").format(limb_name)) 103 | except (socket.error, socket.gaierror): 104 | self._log_networking_error() 105 | return joint_names 106 | 107 | def get_robot_name(self): 108 | """ 109 | Return the name of class of robot from ROS parameter. 110 | 111 | @rtype: str 112 | @return: name of the class of robot (eg. "sawyer", "baxter", etc.) 113 | """ 114 | robot_name = None 115 | try: 116 | robot_name = rospy.get_param("/manifest/robot_class") 117 | except KeyError: 118 | rospy.logerr("RobotParam:get_robot_name cannot detect robot name" 119 | " under param /manifest/robot_class") 120 | except (socket.error, socket.gaierror): 121 | self._log_networking_error() 122 | return robot_name 123 | 124 | def _log_networking_error(self): 125 | msg = ("Failed to connect to the ROS parameter server!\n" 126 | "Please check to make sure your ROS networking is " 127 | "properly configured:\n" 128 | "Intera SDK Networking Reference: {0}\n" 129 | "ROS Networking Reference: {1}").format( 130 | self._sdk_networking_url, 131 | self._ros_networking_url) 132 | self.log_message(msg, "ERROR") 133 | 134 | def log_message(self, msg, level="INFO"): 135 | """ 136 | Print the desired message on stdout using 137 | level-colored text. 138 | 139 | @type msg: str 140 | @param msg: Message text to be desplayed 141 | 142 | @type level: str 143 | @param level: Level to color the text. Valid levels: 144 | ["INFO", "WARN", "ERROR"] 145 | """ 146 | print(("{0}[{1}] {2}{3}").format( 147 | self._color_prefix.format(self._color_dict[level]), 148 | level, msg, self._color_suffix)) 149 | -------------------------------------------------------------------------------- /intera_interface/src/intera_interface/settings.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2013-2018, Rethink Robotics Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | JOINT_ANGLE_TOLERANCE = 0.008726646 16 | HEAD_PAN_ANGLE_TOLERANCE = 0.1 17 | 18 | ## Versioning 19 | SDK_VERSION = '5.3.0' 20 | CHECK_VERSION = True 21 | # Version Compatibility Maps - {current: compatible} 22 | VERSIONS_SDK2ROBOT = {'5.1.0': ['5.1.0', '5.1.1', '5.1.2'], 23 | '5.2.0': ['5.2.0', '5.2.1', '5.2.2'], 24 | '5.3.0': ['5.3.0', '5.3.1', '5.3.2'], 25 | '5.4.0': ['5.4.0', '5.4.1', '5.4.2'] 26 | } 27 | VERSIONS_SDK2GRIPPER = {'5.3.0': 28 | { 29 | 'warn': '2014/5/20 00:00:00', # Version 1.0.0 30 | 'fail': '2013/10/15 00:00:00', # Version 0.6.2 31 | } 32 | } 33 | -------------------------------------------------------------------------------- /intera_interface/src/intera_io/__init__.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2013-2018, Rethink Robotics Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from .io_interface import IODeviceInterface 16 | from .io_command import IOCommand 17 | -------------------------------------------------------------------------------- /intera_interface/src/intera_io/io_command.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2013-2018, Rethink Robotics Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | import json 16 | import rospy 17 | 18 | from intera_core_msgs.msg import IOComponentCommand 19 | 20 | 21 | class IOCommand(object): 22 | ''' 23 | Container for a generic IO command 24 | ''' 25 | def __init__(self, op, args=None, time=None, now=False): 26 | self.time = rospy.Time() if time is None else time 27 | if now: 28 | self.time = rospy.Time.now() 29 | self.op = op 30 | self.args = args if args else {} 31 | 32 | def __str__(self): 33 | return str({"time": self.time, "op": self.op, "args": self.args}) 34 | 35 | def as_msg(self, now=None): 36 | """ 37 | Returns a properly formatted ROS Message that can be used in a Publisher. 38 | 39 | Primarily "stringify"s the json '.args' field and creates an actual 40 | IOComponentCommand ROS Message. Also sets the timestamp if not currently set. 41 | 42 | @type now: bool|None 43 | @param now: sets the '.time' field of the ROS Message. True: set time to now(); 44 | False: reset time to empty (0, 0); Default: set time to now if unset or empty 45 | 46 | @rtype: IOComponentCommand 47 | @return: proper ROS Message to Publish 48 | """ 49 | if now == None: # use time we have OR set to now if we don't have time! 50 | self.time = self.time if not self.time.is_zero() else rospy.Time.now() 51 | elif now == True: # set time to now 52 | self.time = rospy.Time.now() 53 | elif now == False: # reset time to empty (0,0) 54 | self.time = rospy.Time() 55 | 56 | return IOComponentCommand( 57 | time = self.time, 58 | op = self.op, 59 | args = json.dumps(self.args) 60 | ) 61 | 62 | class SetCommand(IOCommand): 63 | ''' 64 | Container for a port or signal set command 65 | ''' 66 | def __init__(self, args=None): 67 | super(SetCommand, self).__init__('set', args) 68 | 69 | def _set(self, components, component_name, 70 | data_type, dimensions, *component_value): 71 | ''' 72 | add a set component command 73 | ''' 74 | self.args.setdefault(components, {}) 75 | self.args[components][component_name] = { 76 | 'format' : {'type' : data_type}, 77 | 'data' : [val for val in component_value] 78 | } 79 | if dimensions > 1: 80 | self.args[components][component_name]['format']['dimensions'] = [dimensions] 81 | 82 | def set_signal(self, signal_name, data_type, *signal_value): 83 | ''' 84 | add a set signal command 85 | ''' 86 | dimensions = len(signal_value) 87 | self._set('signals', signal_name, data_type, dimensions, *signal_value) 88 | return self 89 | 90 | def set_port(self, port_name, data_type, *port_value): 91 | ''' 92 | add a set port command 93 | ''' 94 | dimensions = len(port_value) 95 | self._set('ports', port_name, data_type, dimensions, *port_value) 96 | return self 97 | -------------------------------------------------------------------------------- /intera_interface/src/intera_joint_trajectory_action/__init__.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2013-2018, Rethink Robotics Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from .joint_trajectory_action import JointTrajectoryActionServer 16 | -------------------------------------------------------------------------------- /intera_interface/src/intera_motion_interface/__init__.py: -------------------------------------------------------------------------------- 1 | from .motion_controller_action_client import MotionControllerActionClient 2 | from .motion_trajectory import MotionTrajectory 3 | from .motion_waypoint import MotionWaypoint 4 | from .motion_waypoint_options import MotionWaypointOptions 5 | from .interaction_options import InteractionOptions 6 | from .interaction_publisher import InteractionPublisher 7 | from .random_walk import RandomWalk 8 | from .utility_functions import ( 9 | clamp_float_warn, 10 | ensure_path_to_file_exists, 11 | get_formatted_decimal_string, 12 | is_valid_check_list_for_none, 13 | int2bool, 14 | bool2int, 15 | boolToggle 16 | ) 17 | -------------------------------------------------------------------------------- /intera_interface/src/intera_motion_interface/interaction_publisher.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | # Copyright (c) 2018, Rethink Robotics Inc. 3 | # 4 | # Licensed under the Apache License, Version 2.0 (the "License"); 5 | # you may not use this file except in compliance with the License. 6 | # You may obtain a copy of the License at 7 | # 8 | # http://www.apache.org/licenses/LICENSE-2.0 9 | # 10 | # Unless required by applicable law or agreed to in writing, software 11 | # distributed under the License is distributed on an "AS IS" BASIS, 12 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | # See the License for the specific language governing permissions and 14 | # limitations under the License. 15 | 16 | 17 | import rospy 18 | from intera_core_msgs.msg import InteractionControlCommand 19 | from .interaction_options import InteractionOptions 20 | import intera_interface 21 | from intera_interface import CHECK_VERSION 22 | 23 | class InteractionPublisher(object): 24 | """ 25 | ROS publisher for sending command messages to robot 26 | """ 27 | 28 | def __init__(self): 29 | """ 30 | Constructor - creates a publisher for interaction_control_command 31 | Note, that the program may need to sleep for 0.5 seconds before the 32 | publisher is established. 33 | """ 34 | self.pub = rospy.Publisher('/robot/limb/right/interaction_control_command', 35 | InteractionControlCommand, queue_size=1, 36 | tcp_nodelay=True) 37 | self.enable = intera_interface.RobotEnable(CHECK_VERSION) 38 | 39 | def send_command(self, msg, pub_rate): 40 | """ 41 | @param msg: either an InteractionControlCommand message or 42 | InteractionOptions object to be published 43 | @param pub_rate: the rate in Hz to publish the command 44 | 45 | Note that this function is blocking for non-zero pub_rates until 46 | the node is shutdown (e.g. via cntl+c) or the robot is disabled. 47 | A pub_rate of zero will publish the function once and return. 48 | """ 49 | repeat = False 50 | if pub_rate > 0: 51 | rate = rospy.Rate(pub_rate) 52 | repeat = True 53 | elif pub_rate < 0: 54 | rospy.logerr('Invalid publish rate!') 55 | 56 | if isinstance(msg, InteractionOptions): 57 | msg = msg.to_msg() 58 | 59 | try: 60 | self.pub.publish(msg) 61 | while repeat and not rospy.is_shutdown() and self.enable.state().enabled: 62 | rate.sleep() 63 | self.pub.publish(msg) 64 | except rospy.ROSInterruptException: 65 | rospy.logerr('Keyboard interrupt detected from the user. %s', 66 | 'Exiting the node...') 67 | finally: 68 | if repeat: 69 | self.send_position_mode_cmd() 70 | 71 | def send_position_mode_cmd(self): 72 | ''' 73 | Send a message to put the robot back into position mode 74 | ''' 75 | position_mode = InteractionOptions() 76 | position_mode.set_interaction_control_active(False) 77 | self.pub.publish(position_mode.to_msg()) 78 | rospy.loginfo('Sending position command') 79 | rospy.sleep(0.5) 80 | -------------------------------------------------------------------------------- /intera_interface/src/intera_motion_interface/motion_controller_action_client.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | # Copyright (c) 2016-2018, Rethink Robotics Inc. 3 | # 4 | # Licensed under the Apache License, Version 2.0 (the "License"); 5 | # you may not use this file except in compliance with the License. 6 | # You may obtain a copy of the License at 7 | # 8 | # http://www.apache.org/licenses/LICENSE-2.0 9 | # 10 | # Unless required by applicable law or agreed to in writing, software 11 | # distributed under the License is distributed on an "AS IS" BASIS, 12 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | # See the License for the specific language governing permissions and 14 | # limitations under the License. 15 | 16 | 17 | import rospy 18 | import actionlib 19 | import intera_motion_msgs.msg 20 | 21 | 22 | class MotionControllerActionClient(object): 23 | """ 24 | Simple ROS action client for sending trajectory messages to 25 | the Motion Controller. 26 | """ 27 | 28 | def __init__(self): 29 | """ 30 | Constructor - creates a new motion controller action client and 31 | waits for a connection to the motion controller action server. 32 | """ 33 | self._waypointSequenceId = 0 34 | self._client = actionlib.SimpleActionClient( 35 | '/motion/motion_command', 36 | intera_motion_msgs.msg.MotionCommandAction) 37 | if not self._client.wait_for_server(rospy.Duration(10.0)): 38 | rospy.logerr("Timed out waiting for Motion Controller" 39 | " Server to connect. Check your ROS networking" 40 | " and/or reboot the robot.") 41 | rospy.signal_shutdown("Timed out waiting for Action Server") 42 | 43 | def stop_trajectory(self): 44 | """ 45 | Send a Motion Stop command 46 | """ 47 | goal = intera_motion_msgs.msg.MotionCommandGoal() 48 | goal.command = intera_motion_msgs.msg.MotionCommandGoal.MOTION_STOP 49 | self._client.send_goal(goal) 50 | 51 | def send_trajectory(self, trajectory): 52 | """ 53 | @param trajectory: a Trajectory.msg that will be packaged up with the 54 | MOTION_START command and sent to the motion controller. 55 | @return: (void) immediately after sending 56 | """ 57 | goal = intera_motion_msgs.msg.MotionCommandGoal() 58 | goal.command = intera_motion_msgs.msg.MotionCommandGoal.MOTION_START 59 | goal.trajectory = trajectory 60 | self._client.send_goal(goal) 61 | 62 | def wait_for_result(self, timeout=None): 63 | """ 64 | Wait for the current task to finish and then return the result 65 | @param timeout: maximum time to wait. If timeout is reached, return None. 66 | @return: the result of the MotionCommand.action 67 | """ 68 | if timeout is None: 69 | self._client.wait_for_result() 70 | else: 71 | self._client.wait_for_result(rospy.Duration(timeout)) 72 | return self._client.get_result() 73 | 74 | def get_state(self): 75 | """ 76 | Get the state information of current goal. 77 | @return: the goal's state 78 | """ 79 | return self._client.get_state() 80 | -------------------------------------------------------------------------------- /intera_interface/src/intera_motion_interface/random_walk.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | # Copyright (c) 2016-2018, Rethink Robotics Inc. 3 | # 4 | # Licensed under the Apache License, Version 2.0 (the "License"); 5 | # you may not use this file except in compliance with the License. 6 | # You may obtain a copy of the License at 7 | # 8 | # http://www.apache.org/licenses/LICENSE-2.0 9 | # 10 | # Unless required by applicable law or agreed to in writing, software 11 | # distributed under the License is distributed on an "AS IS" BASIS, 12 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | # See the License for the specific language governing permissions and 14 | # limitations under the License. 15 | 16 | 17 | from copy import deepcopy 18 | import random 19 | import rospy 20 | 21 | class RandomWalk(object): 22 | """ 23 | This class is used to create a random walk in a bounded state space 24 | """ 25 | 26 | def __init__(self, seed = 0): 27 | """ 28 | Constructor - creates an empty random walk object. 29 | Be sure to set the lower limits, upper limits, and past point before 30 | calling getNextPoint(). 31 | """ 32 | 33 | self._lower_limit = None 34 | self._upper_limit = None 35 | self._last_point = None 36 | self._dimension = None 37 | self._boundary_padding = 0.0 38 | self._maximum_distance = 1.0 39 | if seed > 0: 40 | random.seed(seed) 41 | 42 | def set_lower_limits(self, low): 43 | """ 44 | @param low: [float] 45 | @return false if dimensions do not match 46 | @rtype: bool 47 | """ 48 | if not self._check_dimension(low): 49 | return False 50 | self._lower_limit = deepcopy(low) 51 | return True 52 | 53 | def set_upper_limits(self, upp): 54 | """ 55 | @param upp: [float] 56 | @return false if dimensions do not match 57 | @rtype: bool 58 | """ 59 | if not self._check_dimension(upp): 60 | return False 61 | self._upper_limit = deepcopy(upp) 62 | return True 63 | 64 | def set_last_point(self, val): 65 | """ 66 | Set the initial seed point 67 | @param low: val [float] 68 | @return false if dimensions do not match 69 | @rtype: bool 70 | """ 71 | if not self._check_dimension(val): 72 | return False 73 | self._last_point = deepcopy(val) 74 | return True 75 | 76 | def set_boundary_padding(self, pad): 77 | """ 78 | @param pad: bounday padding, must be [0,0.5) 79 | """ 80 | if pad < 0.0: 81 | rospy.logwarn(' cannot set negative padding values') 82 | pad = 0.0 83 | elif pad > 0.499999: 84 | rospy.logwarn(' cannot set padding values greater than 0.5') 85 | pad = 0.499999 86 | self._boundary_padding = pad 87 | 88 | def set_maximum_distance(self, dist): 89 | """ 90 | @param dist: maximum scaled step distance, must be (0,1] 91 | """ 92 | if dist < 0.00001: 93 | rospy.logwarn(' distance must be positive!') 94 | dist = 0.00001 95 | elif dist > 1.0: 96 | rospy.logwarn(' cannot set max distance greater than 1.0') 97 | dist = 1.0 98 | self._maximum_distance = dist 99 | 100 | def get_last_point(self): 101 | """ 102 | @return: the last point used 103 | @rtype: [float] 104 | """ 105 | return deepcopy(self._last_point) 106 | 107 | def get_next_point(self): 108 | """ 109 | @return: ext random point 110 | @rtype: [float] 111 | """ 112 | if self._lower_limit is None: 113 | rospy.logerr('must define lower limits') 114 | return None 115 | elif self._upper_limit is None: 116 | rospy.logerr('must define upper limits') 117 | return None 118 | elif self._last_point is None: 119 | rospy.logerr('must define starting point') 120 | return None 121 | return self._get_next_point() 122 | 123 | def _get_next_point(self): 124 | point = [] 125 | for i in range(0, self._dimension): 126 | low = self._lower_limit[i] 127 | upp = self._upper_limit[i] 128 | val = self._last_point[i] 129 | point.append(self._get_special_bounded_float(low, val, upp)) 130 | self._last_point = point 131 | return deepcopy(point) 132 | 133 | def _get_special_bounded_float(self, low, val, upp): 134 | """ 135 | @param low = hard lower bound on variable 136 | @param upp = hard upper bound on variable 137 | @param val = last sample value 138 | @param pad = normalized passing to apply to boundaries 139 | @param dist = normalized maximum distance from last value 140 | """ 141 | pad = self._boundary_padding 142 | dist = self._maximum_distance 143 | r = upp - low 144 | lower = max(low + pad*r, val - dist*r) 145 | upper = min(upp - pad*r, val + dist*r) 146 | if lower > upper: 147 | print("No valid solution! Ignoring last sample value") 148 | lower = low + pad*r 149 | upper = upp - pad*r 150 | return lower + (upper-lower)*random.random() 151 | 152 | def _check_dimension(self, val): 153 | if self._dimension is None: 154 | self._dimension = len(val) 155 | return True 156 | elif self._dimension != len(val): 157 | rospy.logerr('vector length is not consistent!') 158 | return False 159 | return True 160 | -------------------------------------------------------------------------------- /intera_interface/src/intera_motion_interface/utility_functions.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | # Copyright (c) 2016-2018, Rethink Robotics Inc. 3 | # 4 | # Licensed under the Apache License, Version 2.0 (the "License"); 5 | # you may not use this file except in compliance with the License. 6 | # You may obtain a copy of the License at 7 | # 8 | # http://www.apache.org/licenses/LICENSE-2.0 9 | # 10 | # Unless required by applicable law or agreed to in writing, software 11 | # distributed under the License is distributed on an "AS IS" BASIS, 12 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | # See the License for the specific language governing permissions and 14 | # limitations under the License. 15 | 16 | 17 | import os 18 | import rospy 19 | from copy import deepcopy 20 | 21 | 22 | def get_formatted_decimal_string(i, n): 23 | """ 24 | @param i: index 25 | @param n: total number of indicies 26 | @return: str(i) with correct number of padding zeros 27 | """ 28 | n_digits = len(str(n)) 29 | format_string = '{{:0>{}}}'.format(n_digits) 30 | return format_string.format(i) 31 | 32 | 33 | def ensure_path_to_file_exists(raw_file_path): 34 | """ 35 | This function does two checks: 36 | 1) Expands the ~ in the file path string 37 | 2) Creates any intermediate directories that do not exist 38 | @param raw_file_path 39 | @return: file_name = valid path to file 40 | """ 41 | file_name = os.path.expanduser(raw_file_path) 42 | file_dir = os.path.dirname(file_name) 43 | if file_dir and not os.path.exists(file_dir): 44 | os.makedirs(file_dir) 45 | return file_name 46 | 47 | 48 | def is_valid_check_list_for_none(list_data): 49 | """ 50 | @param list_data: a python list 51 | @return true iff all elements in the list are not None 52 | """ 53 | if None in list_data: 54 | rospy.logwarn("This list contains at least one None value!") 55 | return False 56 | return True 57 | 58 | 59 | def clamp_float_warn(low, val, upp, name): 60 | """ 61 | Clamps: low <= val <= upp 62 | Prints: warning if clamped, error if input is not a float 63 | @param low: lower bound for the input (float) 64 | @param val: input (float ?) 65 | @param upp: upper bound for the input (float) 66 | @param name: input name (str) 67 | @return: clamp(low,val,upp) if input is a float, None otherwise. 68 | """ 69 | if not isinstance(val, float): 70 | rospy.logerr('Invalid input type: ' + name + ' must be a float!') 71 | return None 72 | if val < low: 73 | rospy.logwarn(''.join(['Clamping ' + name, ' (' + str(val) + ')', 74 | ' to the lower bound: ', str(low)])) 75 | return low 76 | if val > upp: 77 | rospy.logwarn(''.join(['Clamping ' + name, ' (' + str(val) + ')', 78 | ' to the upper bound: ', str(upp)])) 79 | return upp 80 | return val 81 | 82 | def int2bool(var): 83 | """ 84 | Convert integer value/list to bool value/list 85 | """ 86 | var_out = deepcopy(var) 87 | 88 | if isinstance(var, int): 89 | var_out = bool(var) 90 | elif len(var) >= 1: 91 | for i in range(0, len(var)): 92 | var_out[i] = bool(var[i]) 93 | 94 | return var_out 95 | 96 | def bool2int(var): 97 | """ 98 | Convert bool value/list to int value/list 99 | """ 100 | var_out = deepcopy(var) 101 | 102 | if isinstance(var, bool): 103 | var_out = int(var) 104 | elif len(var) >= 1: 105 | for i in range(0, len(var)): 106 | var_out[i] = int(var[i]) 107 | 108 | return var_out 109 | 110 | def boolToggle(var): 111 | """ 112 | Toggle bool value/list 113 | """ 114 | var_out = deepcopy(var) 115 | 116 | if isinstance(var, bool): 117 | var_out = not var 118 | elif len(var) >= 1: 119 | for i in range(0, len(var)): 120 | var_out[i] = not var[i] 121 | 122 | return var_out -------------------------------------------------------------------------------- /intera_sdk/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | 5.0.3 (2016-12-05) 2 | --------------------------------- 3 | - Initial release of intera_sdk, borrowing heavily from baxter_sdk 4 | but robot-agnostic to any robot running Intera 5 | -------------------------------------------------------------------------------- /intera_sdk/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(intera_sdk) 3 | find_package(catkin REQUIRED) 4 | catkin_metapackage() 5 | -------------------------------------------------------------------------------- /intera_sdk/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | intera_sdk 4 | 5.3.0 5 | 6 | intera_sdk metapackage containing all of the SDK packages 7 | for Rethink Robotics' robots. 8 | 9 | 10 | 11 | Rethink Robotics Inc. 12 | 13 | Apache 2.0 14 | http://sdk.rethinkrobotics.com/intera/ 15 | 16 | https://github.com/RethinkRobotics/intera_sdk 17 | 18 | 19 | https://github.com/RethinkRobotics/intera_sdk/issues 20 | 21 | Rethink Robotics Inc. 22 | 23 | catkin 24 | 25 | intera_interface 26 | intera_examples 27 | 28 | 29 | 30 | 31 | 32 | 33 | --------------------------------------------------------------------------------