[ { "path": ".github/workflows/docs_pages_workflow.yml", "content": "name: docs_pages_workflow\n \n# execute this workflow automatically when a we push to master\non:\n push:\n branches: [ master ]\n \njobs:\n \n build_docs_job:\n runs-on: ubuntu-latest\n container: ubuntu:focal\n \n steps:\n \n - name: Prereqs\n env:\n GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }}\n run: |\n apt-get update\n apt-get install -y git\n git clone --depth 1 \"https://token:${GITHUB_TOKEN}@github.com/${GITHUB_REPOSITORY}.git\" .\n shell: bash\n \n - name: Setup ROS environment\n uses: ros-tooling/setup-ros@0.2.1\n with:\n required-ros-distributions: noetic\n \n - name: Execute script to build our documentation and update pages\n env:\n GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }}\n run: \"docs/buildDocs.sh\"\n shell: bash\n" }, { "path": ".gitignore", "content": "# Byte-compiled / optimized / DLL files\n__pycache__/\n*.py[cod]\n*$py.class\n\n# C extensions\n*.so\n\n# Distribution / packaging\n.Python\nbuild/\ndevelop-eggs/\ndist/\ndownloads/\neggs/\n.eggs/\nlib/\nlib64/\nparts/\nsdist/\nvar/\nwheels/\npip-wheel-metadata/\nshare/python-wheels/\n*.egg-info/\n.installed.cfg\n*.egg\nMANIFEST\n\n# PyInstaller\n# Usually these files are written by a python script from a template\n# before PyInstaller builds the exe, so as to inject date/other infos into it.\n*.manifest\n*.spec\n\n# Installer logs\npip-log.txt\npip-delete-this-directory.txt\n\n# Unit test / coverage reports\nhtmlcov/\n.tox/\n.nox/\n.coverage\n.coverage.*\n.cache\nnosetests.xml\ncoverage.xml\n*.cover\n*.py,cover\n.hypothesis/\n.pytest_cache/\n\n# Translations\n*.mo\n*.pot\n\n# Django stuff:\n*.log\nlocal_settings.py\ndb.sqlite3\ndb.sqlite3-journal\n\n# Flask stuff:\ninstance/\n.webassets-cache\n\n# Scrapy stuff:\n.scrapy\n\n# PyBuilder\ntarget/\n\n# Jupyter Notebook\n.ipynb_checkpoints\n\n# IPython\nprofile_default/\nipython_config.py\n\n# pyenv\n.python-version\n\n# pipenv\n# According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control.\n# However, in case of collaboration, if having platform-specific dependencies or dependencies\n# having no cross-platform support, pipenv may install dependencies that don't work, or not\n# install all needed dependencies.\n#Pipfile.lock\n\n# PEP 582; used by e.g. github.com/David-OConnor/pyflow\n__pypackages__/\n\n# Celery stuff\ncelerybeat-schedule\ncelerybeat.pid\n\n# SageMath parsed files\n*.sage.py\n\n# Environments\n.env\n.venv\nenv/\nvenv/\nENV/\nenv.bak/\nvenv.bak/\n\n# Spyder project settings\n.spyderproject\n.spyproject\n\n# Rope project settings\n.ropeproject\n\n# mkdocs documentation\n/site\n\n# mypy\n.mypy_cache/\n.dmypy.json\ndmypy.json\n\n# Pyre type checker\n.pyre/\n\n# ROS\ncatkin_ws/devel\ncatkin_ws/build\ncatkin_ws/logs\ncatkin_ws/.catkin_tools" }, { "path": ".gitmodules", "content": "[submodule \"catkin_ws/src/franka-interface-msgs\"]\n\tpath = catkin_ws/src/franka-interface-msgs\n\turl = https://github.com/iamlab-cmu/franka-interface-msgs.git\n" }, { "path": "LICENSE", "content": "MIT License\n\nCopyright (c) 2021 Carnegie Mellon University\n\nPermission is hereby granted, free of charge, to any person obtaining a copy\nof this software and associated documentation files (the \"Software\"), to deal\nin the Software without restriction, including without limitation the rights\nto use, copy, modify, merge, publish, distribute, sublicense, and/or sell\ncopies of the Software, and to permit persons to whom the Software is\nfurnished to do so, subject to the following conditions:\n\nThe above copyright notice and this permission notice shall be included in all\ncopies or substantial portions of the Software.\n\nTHE SOFTWARE IS PROVIDED \"AS IS\", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR\nIMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,\nFITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE\nAUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER\nLIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,\nOUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE\nSOFTWARE.\n" }, { "path": "README.md", "content": "# frankapy\n\nThis is a software package used for controlling and learning skills on the Franka Emika Panda Research Robot Arm.\n\nInstallation Instructions and Network Configuration Instructions are also available here: [https://iamlab-cmu.github.io/frankapy](https://iamlab-cmu.github.io/frankapy)\n\nTo join the Discord community, click the link [here](https://discord.gg/r6r7dttMwZ).\n\n## Requirements\n\n* A computer with the Ubuntu 18.04 / 20.04.\n* ROS Melodic / Noetic\n* [Protocol Buffers](https://github.com/protocolbuffers/protobuf)\n\n## Computer Setup Instructions\n\nThis library is intended to be installed on any computer in the same ROS network with the computer that interfaces with the Franka (we call the latter the Control PC).\n`FrankaPy` will send commands to [franka-interface](https://github.com/iamlab-cmu/franka-interface), which actually controls the robot.\n\n## Install ProtoBuf\n\n**This is only needed if you plan to modify the proto messages. You don't need to install or compile protobuf for using frankapy**\n\nWe use both C++ and Python versions of protobufs so you would need to install Protobufs from source. \n\nDo `nproc` to find out how many cores you have, and use that as the `N` number in the `make` command below:\n\n```shell\nsudo apt-get install autoconf automake libtool curl make g++ unzip\nwget https://github.com/protocolbuffers/protobuf/releases/download/v21.8/protobuf-all-21.8.zip\nunzip protobuf-all-21.8.zip\ncd protobuf-21.8\n./configure\nmake -jN\nmake check -jN\nsudo make install\nsudo ldconfig\n```\nSee detailed instructions [here](https://github.com/protocolbuffers/protobuf/blob/master/src/README.md)\n\n## Installation\n\n1. Clone Repo and its Submodules:\n\n ```bash\n git clone --recurse-submodules git@github.com:iamlab-cmu/frankapy.git \n ```\nAll directories below are given relative to `/frankapy`.\n\n2. First source into your virtualenv or conda env (should be Python 3.6). Then:\n ```bash\n pip install -e .\n ```\n \n3. To compile the catkin_ws use the following script:\n\t```bash\n ./bash_scripts/make_catkin.sh\n ```\n\n4. To allow asynchronous gripper commands, we use the franka\\_ros package, so install libfranka and franka\\_ros using the following command (Change melodic to noetic if you are on Ubuntu 20.04:\n ```bash\n sudo apt install ros-melodic-libfranka ros-melodic-franka-ros\n ```\n\n5. To make the protobufs use the following script (**you don't need to do this if you haven't modified the proto messages**):\n\t```bash\n ./bash_scripts/make_proto.sh\n ```\n\n## Configuring the network with the Control PC\n### Ethernet\n1. If you have an ethernet cable directly between the Control PC and the one sending Frankapy commands, you can go into the Ubuntu Network Connections Menu on the Control PC.\n2. Select the Ethernet connection that corresponds to the port that you plugged the ethernet cable into and then click edit.\n3. Go to the IPv4 Settings Tab and switch from Automatic (DHCP) to Manual.\n4. Add a static ip address like 192.168.1.3 on the Control PC with netmask 24 and then click save.\n5. Then do the same on the FrankaPy PC but instead set the static ip address to be 192.168.1.2.\n\n### Wifi\n1. If you are only communicating with the Control PC over Wifi, use the command `ifconfig` in order to get the wifi ip address of both computers and note them down.\n\n### Editing the /etc/hosts file\n1. Now that you have the ip addresses for both the Control PC and FrankaPy PC, you will need to edit the /etc/hosts files on both in order to allow communication between the 2 over ROS.\n2. On the Control PC, run the command: `sudo gedit /etc/hosts`\n3. If you are using an Ethernet connection, then add the following above the line `# The following lines are desirable for IPv6 capable hosts`:\n ```bash\n 192.168.1.2 [frankapy-pc-name]\n ```\n Otherwise substitute 192.168.1.2 with the ip address of the FrankaPy PC that you discovered using the command `ifconfig`.\n4. Afterwards, on the FrankaPy PC, again run the command `sudo gedit /etc/hosts` and add the line:\n ```bash\n 192.168.1.3 [control-pc-name]\n ```\n Otherwise substitute 192.168.1.3 with the ip address of the Control PC that you discovered using the command `ifconfig`.\n5. Now you should be able to ssh between the FrankaPy PC and the Control PC using the command:\n ```bash\n ssh [control-pc-username]@[control-pc-name]\n Input password to control-pc.\n ```\n\n## Setting Up SSH Key to Control PC\n1. Generate an ssh key by executing the following commands or reading the instructions here: https://help.github.com/en/articles/generating-a-new-ssh-key-and-adding-it-to-the-ssh-agent\n ```bash\n ssh-keygen -t rsa -b 4096 -C \"your_email@example.com\"\n [Press enter]\n [Press enter]\n [Press enter]\n eval \"$(ssh-agent -s)\"\n ssh-add ~/.ssh/id_rsa\n ```\n2. Upload your public ssh key to the control pc.\n 1. In a new terminal, ssh to the control PC.\n ```bash\n ssh [control-pc-username]@[control-pc-name]\n Input password to control-pc.\n ```\n 2. Use your favorite text editor to open the authorized_keys file.\n ```bash\n vim ~/.ssh/authorized_keys\n ```\n 3. In a separate terminal on your Workhorse PC, use your favorite text editor to open your id_rsa.pub file.\n ```bash\n vim ~/.ssh/id_rsa.pub\n ```\n 4. Copy the contents from your id_rsa.pub file to a new line on the authorized_keys file on the Control PC. Then save. \n 5. Open a new terminal and try sshing to the control PC and it should no longer require a password. \n3. (Optional) Upload your ssh key to github by following instructions here: https://help.github.com/en/articles/adding-a-new-ssh-key-to-your-github-account\n\n## Unlocking the Franka Robot\n1. In a new terminal, ssh to the control PC with option -X.\n ```bash\n ssh -X [control-pc-username]@[control-pc-name]\n ```\n2. Open a web browser, either firefox or google chrome.\n ```bash\n firefox\n ```\n3. Go to 172.16.0.2 in the web browser.\n4. (Optional) Input the username admin and the password to login to the Franka Desk GUI.\n5. Unlock the robot by clicking the unlock button on the bottom right of the web interface.\n6. If the robot has pink lights, press down on the e-stop and then release it and the robot should turn blue. If the robot is white, just release the e-stop and it should also turn blue.\n\n## Running the Franka Robot\n\n1. Make sure that both the user stop and the brakes of the Franka robot have been unlocked in the Franka Desk GUI.\n2. Open up a new terminal and go to the frankapy directory.\n ```bash\n bash ./bash_scripts/start_control_pc.sh -i [control-pc-name]\n ```\n Please see the `start_control_pc.sh` bash script for additional arguments, including specifying a custom directory for where `franka-interface` is installed on the Control PC as well as the username of the account on the Control PC. By default the username is `iam-lab`.\n \n3. Open up a new terminal and go to the frankapy directory. Do:\n ```bash\n source catkin_ws/devel/setup.bash\n ```\n Be in the same virtualenv or Conda env that FrankaPy was installed in. Place your hand on top of the e-stop. Reset the robot pose with the following command.\n \n ```bash\n python scripts/reset_arm.py\n ```\n \nSee example scripts in the `examples/` and `scripts/` to learn how to use the `FrankaPy` python package.\n\n## Citation\n\nIf this library proves useful to your research, please cite the paper below::\n```\n@article{zhang2020modular,\ntitle={A modular robotic arm control stack for research: Franka-interface and frankapy},\nauthor={Zhang, Kevin and Sharma, Mohit and Liang, Jacky and Kroemer, Oliver},\njournal={arXiv preprint arXiv:2011.02398},\nyear={2020}\n}\n```\n" }, { "path": "bash_scripts/make_catkin.sh", "content": "cd catkin_ws\ncatkin build\ncd .." }, { "path": "bash_scripts/make_proto.sh", "content": "protoc -I=frankapy/proto/ --python_out=frankapy/proto/ frankapy/proto/*.proto" }, { "path": "bash_scripts/start_control_pc.sh", "content": "#!/bin/bash\n\ndie () {\n echo >&2 \"$@\"\n exit 1\n}\n\nusage=\"$(basename \"$0\") [-h] [-i xxx.xxx.xxx.xxx] -- Start control PC from workstation\n\nwhere:\n -h show this help text\n -i IP address for the Control PC. If you use localhost, it will treat the current PC as the Control PC.\n -u Username on control PC. (default iam-lab)\n -p Control PC password\n -d Path to franka_interface on Control PC (default Documents/franka-interface)\n -r Robot number (default 1)\n -a Robot IP address (default 172.16.0.2)\n -s Start franka-interface on Control PC (0 / 1 (default))\n -g Robot has gripper (0 / 1 (default))\n -o Using old gripper commands (0 (default) / 1)\n -l Log at 1kHz on franka-interface (0 (default) / 1)\n -e Stop franka-interface when an error has occurred (0 (default) / 1)\n \n ./start_control_pc.sh -i iam-space\n ./start_control_pc.sh -i iam-space -u iam-lab -p 12345678 -d ~/Documents/franka-interface -r 1 -s 0\n \"\n\ncontrol_pc_uname=\"iam-lab\"\ncontrol_pc_use_password=0\ncontrol_pc_password=\"\"\ncontrol_pc_franka_interface_path=\"Documents/franka-interface\"\nstart_franka_interface=1\nrobot_number=1\nrobot_ip=\"172.16.0.2\"\nwith_gripper=1\nold_gripper=0\nlog_on_franka_interface=0\nstop_on_error=0\n\nwhile getopts ':h:i:u:p:d:r:a:s:g:o:l:e' option; do\n case \"${option}\" in\n h) echo \"$usage\"\n exit\n ;;\n i) control_pc_ip_address=$OPTARG\n ;;\n u) control_pc_uname=$OPTARG\n ;;\n p) control_pc_password=$OPTARG\n control_pc_use_password=1\n ;;\n d) control_pc_franka_interface_path=$OPTARG\n ;;\n r) robot_number=$OPTARG\n ;;\n a) robot_ip=$OPTARG\n ;;\n s) start_franka_interface=$OPTARG\n ;;\n g) with_gripper=$OPTARG\n ;;\n o) old_gripper=$OPTARG\n ;;\n l) log_on_franka_interface=$OPTARG\n ;;\n e) stop_on_error=$OPTARG\n ;;\n :) printf \"missing argument for -%s\\n\" \"$OPTARG\" >&2\n echo \"$usage\" >&2\n exit 1\n ;;\n \\?) printf \"illegal option: -%s\\n\" \"$OPTARG\" >&2\n echo \"$usage\" >&2\n exit 1\n ;;\n esac\ndone\nshift $((OPTIND - 1))\n\nworkstation_ip_address=\"`hostname`\"\n\n# Notify the IP addresses being used.\necho \"Control PC IP uname/address: \"$control_pc_uname\"@\"$control_pc_ip_address\necho \"Workstation IP address: \"$workstation_ip_address\nif [ \"$control_pc_use_password\" -eq 0 ]; then\n echo \"Will not use password to ssh into control pc.\"\nelse\n echo \"Will use input password to ssh into control pc.\"\nfi\n\nDIR=\"$( cd \"$( dirname \"${BASH_SOURCE[0]}\" )\" && pwd )\"\n\n# Start rosmaster in a new gnome-terminal if not already running\nif ! pgrep -x \"roscore\" > /dev/null\nthen\n start_rosmaster_path=\"$DIR/start_rosmaster.sh\"\n echo \"Will start ROS master in new terminal.\"$start_rosmaster_path\n gnome-terminal --working-directory=\"$DIR\" -- bash $start_rosmaster_path\n sleep 3\n echo \"Did start ROS master in new terminal.\"\nelse\n echo \"Roscore is already running\"\nfi\n\nif [ \"$with_gripper\" -eq 0 ]; then\nlet old_gripper=0\nfi\n\nif [ \"$start_franka_interface\" -eq 1 ]; then\n# ssh to the control pc and start franka_interface in a new gnome-terminal\nstart_franka_interface_on_control_pc_path=\"$DIR/start_franka_interface_on_control_pc.sh\"\necho \"Will ssh to control PC and start franka-interface.\"\ngnome-terminal --working-directory=\"$DIR\" -- bash $start_franka_interface_on_control_pc_path $robot_ip $old_gripper $log_on_franka_interface $stop_on_error $control_pc_uname $control_pc_ip_address $control_pc_franka_interface_path $control_pc_use_password $control_pc_password \necho \"Done\"\nsleep 3\nelse\necho \"Will not start franka-interface on the control pc.\"\nfi\n\n# ssh to the control pc and start ROS action server in a new gnome-terminal\nstart_franka_ros_interface_on_control_pc_path=\"$DIR/start_franka_ros_interface_on_control_pc.sh\"\necho \"Will ssh to control PC and start ROS action server.\"\ngnome-terminal --working-directory=\"$DIR\" -- bash $start_franka_ros_interface_on_control_pc_path $control_pc_uname $control_pc_ip_address $workstation_ip_address $control_pc_franka_interface_path $robot_number $control_pc_use_password $control_pc_password\nsleep 3\n\nif [ \"$with_gripper\" -eq 1 ] && [ \"$old_gripper\" -eq 0 ]; then\nstart_franka_gripper_on_control_pc_path=\"$DIR/start_franka_gripper_on_control_pc.sh\"\necho \"Will ssh to control PC and start ROS action server.\"\ngnome-terminal --working-directory=\"$DIR\" -- bash $start_franka_gripper_on_control_pc_path $control_pc_uname $control_pc_ip_address $workstation_ip_address $control_pc_franka_interface_path $robot_number $robot_ip $control_pc_use_password $control_pc_password\nsleep 3\nelse\n echo \"Will not start franka gripper on the control pc.\"\nfi\n\necho \"Done\"\n" }, { "path": "bash_scripts/start_franka_gripper_on_control_pc.sh", "content": "#!/bin/bash\n\ncontrol_pc_uname=${1}\ncontrol_pc_ip_address=${2}\nworkstation_ip_address=${3}\ncontrol_pc_franka_interface_path=${4}\nrobot_number=${5}\nrobot_ip=${6}\ncontrol_pc_use_password=${7}\ncontrol_pc_password=${8}\n\nrosmaster_path=\"bash_scripts/set_rosmaster.sh\"\ncatkin_ws_setup_path=\"catkin_ws/devel/setup.bash\"\n\nif [ \"$control_pc_ip_address\" = \"localhost\" ]; then\n cd $HOME\n cd $control_pc_franka_interface_path\n source $catkin_ws_setup_path\n roslaunch franka_ros_interface franka_gripper.launch robot_num:=$robot_number robot_ip:=$robot_ip\n bash\nelse\nif [ \"$control_pc_use_password\" = \"0\" ]; then\nssh -tt $control_pc_uname@$control_pc_ip_address << EOSSH\ncd $control_pc_franka_interface_path\nsource $rosmaster_path $control_pc_ip_address $workstation_ip_address\nsource $catkin_ws_setup_path\nroslaunch franka_ros_interface franka_gripper.launch robot_num:=$robot_number robot_ip:=$robot_ip\nbash\nEOSSH\nelse\nsshpass -p \"$control_pc_password\" ssh -tt -o StrictHostKeyChecking=no $control_pc_uname@$control_pc_ip_address << EOSSH\ncd $control_pc_franka_interface_path\nsource $rosmaster_path $control_pc_ip_address $workstation_ip_address\nsource $catkin_ws_setup_path\nroslaunch franka_ros_interface franka_gripper.launch robot_num:=$robot_number robot_ip:=$robot_ip\nbash\nEOSSH\nfi\nfi" }, { "path": "bash_scripts/start_franka_interface_on_control_pc.sh", "content": "#!/bin/bash\n\nrobot_ip=${1}\nwith_gripper=${2}\nlog_on_franka_interface=${3}\nstop_on_error=${4}\ncontrol_pc_uname=${5}\ncontrol_pc_ip_address=${6}\ncontrol_pc_franka_interface_path=${7}\ncontrol_pc_use_password=${8}\ncontrol_pc_password=${9}\nfranka_interface_stop_on_error=${10}\n\n\nif [ \"$control_pc_ip_address\" = \"localhost\" ]; then\n cd $HOME\n cd $control_pc_franka_interface_path\n cd build\n ./franka_interface --robot_ip $robot_ip --with_gripper $with_gripper --log $log_on_franka_interface --stop_on_error $stop_on_error\n bash\nelse\nif [ \"$control_pc_use_password\" = \"0\" ]; then\nssh -tt $control_pc_uname@$control_pc_ip_address << EOSSH\ncd $control_pc_franka_interface_path\ncd build\n./franka_interface --robot_ip $robot_ip --with_gripper $with_gripper --log $log_on_franka_interface --stop_on_error $stop_on_error\nbash\nEOSSH\nelse\nsshpass -p \"$control_pc_password\" ssh -tt -o StrictHostKeyChecking=no $control_pc_uname@$control_pc_ip_address << EOSSH\ncd $control_pc_franka_interface_path\ncd build\necho $stop_on_error\n./franka_interface --robot_ip $robot_ip --with_gripper $with_gripper --log $log_on_franka_interface --stop_on_error $stop_on_error\nbash\nEOSSH\nfi\nfi" }, { "path": "bash_scripts/start_franka_ros_interface_on_control_pc.sh", "content": "#!/bin/bash\n\ncontrol_pc_uname=${1}\ncontrol_pc_ip_address=${2}\nworkstation_ip_address=${3}\ncontrol_pc_franka_interface_path=${4}\nrobot_number=${5}\ncontrol_pc_use_password=${6}\ncontrol_pc_password=${7}\n\nrosmaster_path=\"bash_scripts/set_rosmaster.sh\"\ncatkin_ws_setup_path=\"catkin_ws/devel/setup.bash\"\n\nif [ \"$control_pc_ip_address\" = \"localhost\" ]; then\n cd $HOME\n cd $control_pc_franka_interface_path\n source $catkin_ws_setup_path\n roslaunch franka_ros_interface franka_ros_interface.launch robot_num:=$robot_number\n bash\nelse\nif [ \"$control_pc_use_password\" = \"0\" ]; then\nssh -tt $control_pc_uname@$control_pc_ip_address << EOSSH\ncd $control_pc_franka_interface_path\nsource $rosmaster_path $control_pc_ip_address $workstation_ip_address\nsource $catkin_ws_setup_path\nroslaunch franka_ros_interface franka_ros_interface.launch robot_num:=$robot_number\nbash\nEOSSH\nelse\nsshpass -p \"$control_pc_password\" ssh -tt -o StrictHostKeyChecking=no $control_pc_uname@$control_pc_ip_address << EOSSH\ncd $control_pc_franka_interface_path\nsource $rosmaster_path $control_pc_ip_address $workstation_ip_address\nsource $catkin_ws_setup_path\nroslaunch franka_ros_interface franka_ros_interface.launch robot_num:=$robot_number\nbash\nEOSSH\nfi\nfi\n" }, { "path": "bash_scripts/start_rosmaster.sh", "content": "#!/bin/bash\n\nroscore" }, { "path": "cfg/april_tag_pick_place_cfg.yaml", "content": "rs:\n id: 0\n frame: realsense\n filter_depth: True\n\napril_tag:\n detector:\n families: tag36h11\n border: 1\n nthreads: 4\n quad_decimate: 1.0\n quad_blur: 0.0\n refine_edges: True\n refine_decode: False\n refine_pose: False\n debug: False\n quad_contours: True\n tag_size: 0.0254 # 1 inch in meters \n\nT_camera_ee_path: examples/T_RSD415_franka.tf\nT_camera_mount_path: examples/T_RS_mount_delta.tf\n\ncube_size: 0.04\n\nvis_detect: True\n" }, { "path": "docs/.gitignore", "content": "*.swp\n/_build\n/doctrees\n" }, { "path": "docs/Makefile", "content": "# Minimal makefile for Sphinx documentation\n#\n\n# You can set these variables from the command line, and also\n# from the environment for the first two.\nSPHINXOPTS ?=\nSPHINXBUILD ?= sphinx-build\nSOURCEDIR = .\nBUILDDIR = _build\n\n# Put it first so that \"make\" without argument is like \"make help\".\nhelp:\n\t@$(SPHINXBUILD) -M help \"$(SOURCEDIR)\" \"$(BUILDDIR)\" $(SPHINXOPTS) $(O)\n\n.PHONY: help Makefile\n\n# Catch-all target: route all unknown targets to Sphinx using the new\n# \"make mode\" option. $(O) is meant as a shortcut for $(SPHINXOPTS).\n%: Makefile\n\t@$(SPHINXBUILD) -M $@ \"$(SOURCEDIR)\" \"$(BUILDDIR)\" $(SPHINXOPTS) $(O)\n" }, { "path": "docs/buildDocs.sh", "content": "#!/bin/bash\nset -x\n################################################################################\n# File: buildDocs.sh\n# Purpose: Script that builds our documentation using sphinx and updates GitHub\n# Pages. This script is executed by:\n# .github/workflows/docs_pages_workflow.yml\n#\n# Authors: Michael Altfield \n# Created: 2020-07-17\n# Updated: 2020-07-17\n# Version: 0.1\n################################################################################\n \n###################\n# INSTALL DEPENDS #\n###################\n \napt-get update\napt-get -y install git rsync python3-sphinx python3-sphinx-rtd-theme python3-setuptools python3-pip\napt-get -y install ros-noetic-libfranka ros-noetic-franka-ros\n\nsudo rosdep init\nrosdep update\n\nsudo -H pip3 install --upgrade pip\nsudo -H pip3 install --upgrade packaging>=24 ordered-set>=3.1.1 more_itertools>=8.8 jaraco.text>=3.7 importlib_resources>=5.10.2 importlib_metadata>=6 tomli>=2.0.1 wheel>=0.43.0 platformdirs>=2.6.2\nsudo -H pip3 install .\n\nsource /opt/ros/noetic/setup.bash\n\ngit config --global --add safe.directory /__w/frankapy/frankapy\ngit submodule update --init --recursive\n\ncd catkin_ws\ncatkin build\nsource devel/setup.bash\ncd ..\n \n#####################\n# DECLARE VARIABLES #\n#####################\n \npwd\nls -lah\nexport SOURCE_DATE_EPOCH=$(git log -1 --pretty=%ct)\n \n##############\n# BUILD DOCS #\n##############\n \n# build our documentation with sphinx (see docs/conf.py)\n# * https://www.sphinx-doc.org/en/master/usage/quickstart.html#running-the-build\nmake -C docs clean\nmake -C docs html\n \n#######################\n# Update GitHub Pages #\n#######################\n \ngit config --global user.name \"${GITHUB_ACTOR}\"\ngit config --global user.email \"${GITHUB_ACTOR}@users.noreply.github.com\"\n \ndocroot=`mktemp -d`\nrsync -av \"docs/_build/html/\" \"${docroot}/\"\n \npushd \"${docroot}\"\n \n# don't bother maintaining history; just generate fresh\ngit init\ngit remote add deploy \"https://token:${GITHUB_TOKEN}@github.com/${GITHUB_REPOSITORY}.git\"\ngit checkout -b gh-pages\n \n# add .nojekyll to the root so that github won't 404 on content added to dirs\n# that start with an underscore (_), such as our \"_content\" dir..\ntouch .nojekyll\n \n# Add README\ncat > README.md <`_. \n\nIf this library proves useful to your research, please cite the paper below::\n\n @article{zhang2020modular,\n title={A modular robotic arm control stack for research: Franka-interface and frankapy},\n author={Zhang, Kevin and Sharma, Mohit and Liang, Jacky and Kroemer, Oliver},\n journal={arXiv preprint arXiv:2011.02398},\n year={2020}\n }\n\nNote that this library has been released with the Apache v2.0 license.\n\n.. toctree::\n :maxdepth: 2\n \n install\n network\n running\n frankaarm\n support\n\nIndices and tables\n==================\n\n* :ref:`genindex`\n* :ref:`modindex`\n* :ref:`search`\n" }, { "path": "docs/install.rst", "content": "Installation\n============\n\nRequirements\n------------\n\n* A computer with Ubuntu 18.04 / 20.04\n* ROS Melodic / Noetic\n\n\nSteps\n-----\n\n1. Clone the Frankapy Repository and its Submodules::\n\n git clone --recurse-submodules git@github.com:iamlab-cmu/frankapy.git\n\n2. To allow asynchronous gripper commands, we use the ``franka_ros`` package, so install libfranka and franka_ros using the following command (Change melodic to noetic if you are on Ubuntu 20.04)::\n\n sudo apt install ros-melodic-libfranka ros-melodic-franka-ros\n\n3. Create and enter a virtual environment (:ref:`Virtual Environment`) and then run the following commands::\n\n cd frankapy\n pip install -e .\n\n4. Compile the catkin_ws using the following script::\n\n ./bash_scripts/make_catkin.sh\n\n5. Afterwards source the ``catkin_ws`` using the following command::\n\n source catkin_ws/devel/setup.bash\n\n6. It is a good idea to add the following line to the end of your ``~/.bashrc`` file::\n\n source /path/to/frankapy/catkin_ws/devel/setup.bash --extend\n\n\n(Optional) Additional Steps\n---------------------------\n\nProtobuf\n~~~~~~~~\n\nIf you plan on modifying the library and the protobuf messages, you will need to compile the `Google Protocol Buffer `_ library from scratch using the following instructions.\n\n1. First determine the number of cores on your computer using the command::\n\n nproc\n\n2. Execute the following commands::\n\n sudo apt-get install autoconf automake libtool curl make g++ unzip\n wget https://github.com/protocolbuffers/protobuf/releases/download/v21.8/protobuf-all-21.8.zip\n unzip protobuf-all-21.8.zip\n cd protobuf-21.8\n ./configure\n\n3. Use the number that was previously printed out using the ``nproc`` command above and substitute it as ``N`` below::\n\n make -jN\n sudo make install\n sudo ldconfig\n\n4. Afterwards, you can make the protobuf messages using the following script::\n\n ./bash_scripts/make_proto.sh\n\n\nVirtual Environment\n~~~~~~~~~~~~~~~~~~~\n\nNote that these instructions work on Ubuntu 18.04. They might be slightly different for Ubuntu 20.04.\n\n1. Install Python3.6::\n\n sudo apt install -y python3-distutils\n\n2. Install Pip::\n\n curl https://bootstrap.pypa.io/get-pip.py | sudo -H python3.6\n\n3. Install Virtual Environment and Other Useful Python Packages::\n\n sudo -H pip3.6 install numpy matplotlib virtualenv\n\n4. Create a Virtual Environment for Frankapy::\n\n virtualenv -p python3.6 franka\n\n5. Enter into the Virtual Environment::\n\n source franka/bin/activate\n\n6. How to exit the Virtual Environment::\n\n deactivate\n" }, { "path": "docs/make.bat", "content": "@ECHO OFF\r\n\r\npushd %~dp0\r\n\r\nREM Command file for Sphinx documentation\r\n\r\nif \"%SPHINXBUILD%\" == \"\" (\r\n\tset SPHINXBUILD=sphinx-build\r\n)\r\nset SOURCEDIR=.\r\nset BUILDDIR=_build\r\n\r\nif \"%1\" == \"\" goto help\r\n\r\n%SPHINXBUILD% >NUL 2>NUL\r\nif errorlevel 9009 (\r\n\techo.\r\n\techo.The 'sphinx-build' command was not found. Make sure you have Sphinx\r\n\techo.installed, then set the SPHINXBUILD environment variable to point\r\n\techo.to the full path of the 'sphinx-build' executable. Alternatively you\r\n\techo.may add the Sphinx directory to PATH.\r\n\techo.\r\n\techo.If you don't have Sphinx installed, grab it from\r\n\techo.http://sphinx-doc.org/\r\n\texit /b 1\r\n)\r\n\r\n%SPHINXBUILD% -M %1 %SOURCEDIR% %BUILDDIR% %SPHINXOPTS% %O%\r\ngoto end\r\n\r\n:help\r\n%SPHINXBUILD% -M help %SOURCEDIR% %BUILDDIR% %SPHINXOPTS% %O%\r\n\r\n:end\r\npopd\r\n" }, { "path": "docs/network.rst", "content": "Network Configuration\n=====================\n\nIf you are running franka-interface and frankapy on the same computer, you can skip this page.\n\nRequirements\n------------\n\n* A Control PC computer running Ubuntu 18.04 with a Realtime Kernel. It should also have franka-interface installed with ROS Melodic.\n\nTerminology\n-----------\n\nControl PC - Realtime Kernel PC connected by Ethernet to the Robot\n\nFrankaPy PC - Computer running FrankaPy\n\nEthernet\n--------\n\n1. Plug an ethernet cable directly between the Control PC and the FrankaPy PC.\n\n2. Go into the Ubuntu Network Connections Menu on the Control PC.\n\n3. Select the Wired connection that corresponds to the Ethernet port that is connected to the FrankaPy PC. Then click the settings gear icon to edit.\n\n4. Go to the IPv4 Settings Tab and switch from Automatic (DHCP) to Manual. Add a static ip address like 192.168.1.3 on the Control PC with netmask 24 and then click Apply.\n\n .. image:: imgs/network_config.png\n :width: 600\n :alt: Network Config Photo\n\n5. Then do the same on the FrankaPy PC but instead set the static ip address to be 192.168.1.2 with netmask 24.\n\nWifi\n----\n\nWhile FrankaPy will work over Wifi, it is not recommended due to additional latency in sending commands between computers.\n\n1. If you are only communicating with the Control PC over Wifi, use the command ``ifconfig`` in order to get the Wifi IP address of both the Control PC and FrankaPy PC and note them down.\n\nEditing the /etc/hosts file\n---------------------------\n\n1. Now that you have the IP addresses for both the Control PC and FrankaPy PC, you will need to edit the /etc/hosts files on both computers in order to allow communication between the 2 over ROS.\n\n2. On the Control PC, run the command::\n\n sudo gedit /etc/hosts\n\n3. If you are using an Ethernet connection, then add the following above the line ``# The following lines are desirable for IPv6 capable hosts:`` ::\n\n 192.168.1.2 [frankapy-pc-name]\n\n\\ \n Otherwise substitute ``192.168.1.2`` with the IP address of the FrankaPy PC that you discovered using the command ``ifconfig``.\n\n4. Afterwards, on the FrankaPy PC, again run the command ``sudo gedit /etc/hosts`` and add the line::\n\n 192.168.1.3 [control-pc-name]\n\n\\\n Otherwise substitute ``192.168.1.3`` with the IP address of the Control PC that you discovered using the command ``ifconfig``.\n\n5. Now you should be able to ssh from the FrankaPy PC to the Control PC using the command::\n\n ssh [control-pc-username]@[control-pc-name]\n Input password to control-pc.\n\nSetting Up SSH Key to Control PC\n--------------------------------\n\nGenerate a new SSH Key on the FrankaPy PC\n~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~\n1. Generate a SSH key by executing the following commands or reading these `instructions `_::\n\n ssh-keygen -t rsa -b 4096 -C \"your_email@example.com\"\n [Press enter]\n [Press enter]\n [Press enter]\n eval \"$(ssh-agent -s)\"\n ssh-add ~/.ssh/id_rsa\n\nUpload the public SSH key to the Control PC\n~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~\n\n1. In a new terminal on your FrankaPy PC, use your favorite text editor to open your ``id_rsa.pub`` file::\n\n gedit ~/.ssh/id_rsa.pub\n\n2. Copy the contents in your ``id_rsa.pub`` file.\n\n3. Next, SSH to the Control PC::\n\n ssh [control-pc-username]@[control-pc-name]\n Input password to control-pc.\n\n4. Use vim to open the authorized_keys file::\n\n vim ~/.ssh/authorized_keys\n\n5. Press the following buttons to paste your copied public key into the ``authorized_keys`` file on the Control PC::\n\n i\n ctrl-shift-v\n \n :\n w\n q\n \n\n6. Open a new terminal on the FrankaPy PC and try SSHing to the Control PC and it should no longer require a password.\n \n7. (Optional) Upload your SSH key to Github by following instructions `here `_." }, { "path": "docs/running.rst", "content": "Running the Robot\n=================\n\nUnlocking the Franka Robot\n--------------------------\n\n1. If you are running franka-interface and frankapy on the same computer, you can skip to step 2. If you have a FrankaPy PC and a Control PC, first ssh to the Control PC from the FrankaPy PC using SSH with option ``-X``::\n\n ssh -X [control-pc-username]@[control-pc-name]\n\n2. Open a web browser, either firefox or google chrome using the command line::\n \n firefox\n\n3. Go to ``172.16.0.2`` in the web browser.\n\n4. Login to the Franka Desk GUI using the username and password that you used during the initial robot setup.\n\n5. Unlock the robot by clicking the unlock button on the bottom right of the web interface.\n\n6. If the robot has pink lights, press down on the e-stop and then release it and the robot should turn blue. If the robot is white, just release the e-stop and it should also turn blue.\n\n\nStarting the FrankaPy Interface\n-------------------------------\n\n1. Make sure that the Franka Robot has been unlocked in the Franka Desk GUI and has blue lights. \n\n2. Open up a new terminal and go to the frankapy directory.\n\n3. If you are running franka-interface and frankapy on the same computer, run the following command::\n\n bash ./bash_scripts/start_control_pc.sh -i localhost\n\n4. Otherwise run the following command::\n\n bash ./bash_scripts/start_control_pc.sh -u [control-pc-username] -i [control-pc-name]\n\n5. Please see the ``bash_scripts/start_control_pc.sh`` bash script for additional arguments, including specifying a custom directory for where franka-interface is installed on the Control PC. \n\n6. Open up a new terminal, enter into the same virtual environment that FrankaPy was installed in, go to the frankapy directory, then::\n\n source catkin_ws/devel/setup.bash\n\n7. Place your hand on top of the e-stop and reset the robot with the following command::\n\n python scripts/reset_arm.py\n\n8. See example scripts in the ``examples/`` and ``scripts/`` folders to learn how to use the FrankaPy python package.\n\n9. Please note that if you are using a custom gripper or no gripper, please set the ``with_gripper=True`` flag in ``frankapy/franka_arm.py`` to ``False`` as well as set the ``with_gripper=1`` flag in ``bash_scripts/start_control_pc.sh`` to ``0``." }, { "path": "docs/support.rst", "content": "Support\n=======\n\nThe easiest way to get help with the library is to join the FrankaPy and Franka-Interface Discord using this link `here `_.\n" }, { "path": "examples/T_RSD415_franka.tf", "content": "realsense\nfranka_tool\n-0.046490 0.030720 -0.083270\n-0.000000 1.000000 0.000000\n-1.000000 -0.000000 0.000000\n0.000000 0.000000 1.000000\n" }, { "path": "examples/T_RS_mount_delta.tf", "content": "franka_tool\nfranka_tool_base\n0. 0. -0.005\n1. 0. 0.\n0. 1. 0.\n0. 0. 1." }, { "path": "examples/april_tag_pick_place.py", "content": "import os\nimport logging\nimport argparse\nfrom time import sleep\n\nimport numpy as np\n\nfrom autolab_core import RigidTransform, YamlConfig\nfrom visualization import Visualizer3D as vis3d\n\nfrom perception_utils.apriltags import AprilTagDetector\nfrom perception_utils.realsense import get_first_realsense_sensor\n\nfrom frankapy import FrankaArm\n\n\ndef subsample(data, rate=0.1):\n idx = np.random.choice(np.arange(len(data)), size=int(rate * len(data)))\n return data[idx]\n\n\ndef make_det_one(R):\n U, _, Vt = np.linalg.svd(R)\n return U @ np.eye(len(R)) @ Vt\n\n\ndef get_closest_grasp_pose(T_tag_world, T_ee_world):\n tag_axes = [\n T_tag_world.rotation[:,0], -T_tag_world.rotation[:,0],\n T_tag_world.rotation[:,1], -T_tag_world.rotation[:,1]\n ]\n x_axis_ee = T_ee_world.rotation[:,0]\n dots = [axis @ x_axis_ee for axis in tag_axes]\n grasp_x_axis = tag_axes[np.argmax(dots)]\n grasp_z_axis = np.array([0, 0, -1])\n grasp_y_axis = np.cross(grasp_z_axis, grasp_x_axis)\n grasp_R = make_det_one(np.c_[grasp_x_axis, grasp_y_axis, grasp_z_axis])\n grasp_translation = T_tag_world.translation + np.array([0, 0, -cfg['cube_size'] / 2])\n return RigidTransform(\n rotation=grasp_R,\n translation=grasp_translation,\n from_frame=T_ee_world.from_frame, to_frame=T_ee_world.to_frame\n )\n\n\nif __name__ == \"__main__\":\n logging.getLogger().setLevel(logging.INFO)\n parser = argparse.ArgumentParser()\n parser.add_argument('--cfg', '-c', type=str, default='cfg/april_tag_pick_place_cfg.yaml')\n parser.add_argument('--no_grasp', '-ng', action='store_true')\n args = parser.parse_args()\n cfg = YamlConfig(args.cfg)\n T_camera_ee = RigidTransform.load(cfg['T_camera_ee_path'])\n T_camera_mount_delta = RigidTransform.load(cfg['T_camera_mount_path'])\n\n logging.info('Starting robot')\n fa = FrankaArm()\n fa.set_tool_delta_pose(T_camera_mount_delta)\n fa.reset_joints()\n fa.open_gripper()\n\n T_ready_world = fa.get_pose()\n T_ready_world.translation[0] += 0.25\n T_ready_world.translation[2] = 0.4\n\n fa.goto_pose(T_ready_world)\n\n logging.info('Init camera')\n sensor = get_first_realsense_sensor(cfg['rs'])\n sensor.start()\n\n logging.info('Detecting April Tags')\n april = AprilTagDetector(cfg['april_tag'])\n intr = sensor.color_intrinsics\n T_tag_camera = april.detect(sensor, intr, vis=cfg['vis_detect'])[0]\n T_camera_world = T_ready_world * T_camera_ee\n T_tag_world = T_camera_world * T_tag_camera\n logging.info('Tag has translation {}'.format(T_tag_world.translation))\n\n logging.info('Finding closest orthogonal grasp')\n T_grasp_world = get_closest_grasp_pose(T_tag_world, T_ready_world)\n T_lift = RigidTransform(translation=[0, 0, 0.2], from_frame=T_ready_world.to_frame, to_frame=T_ready_world.to_frame)\n T_lift_world = T_lift * T_grasp_world\n\n logging.info('Visualizing poses')\n _, depth_im, _ = sensor.frames()\n points_world = T_camera_world * intr.deproject(depth_im)\n\n if cfg['vis_detect']:\n vis3d.figure()\n vis3d.pose(RigidTransform())\n vis3d.points(subsample(points_world.data.T), color=(0,1,0), scale=0.002)\n vis3d.pose(T_ready_world)\n vis3d.pose(T_camera_world)\n vis3d.pose(T_tag_world)\n vis3d.pose(T_grasp_world)\n vis3d.pose(T_lift_world)\n vis3d.show()\n\n if not args.no_grasp:\n logging.info('Commanding robot')\n fa.goto_pose(T_lift_world, use_impedance=False)\n fa.goto_pose(T_grasp_world, use_impedance=False)\n fa.close_gripper()\n fa.goto_pose(T_lift_world, use_impedance=False)\n sleep(3)\n fa.goto_pose(T_grasp_world, use_impedance=False)\n fa.open_gripper()\n fa.goto_pose(T_lift_world, use_impedance=False)\n fa.goto_pose(T_ready_world, use_impedance=False)\n\n import IPython; IPython.embed(); exit(0)\n" }, { "path": "examples/example_movements.py", "content": "from frankapy import FrankaArm\nimport numpy as np\nfrom autolab_core import RigidTransform\n\nif __name__ == '__main__':\n\n print('Starting robot')\n fa = FrankaArm()\n\n xtranslation_3cm = RigidTransform(rotation=np.array([\n [1, 0, 0],\n [0, 1, 0],\n [0, 0, 1]\n ]), translation=np.array([0.03, 0, 0]),\n from_frame='franka_tool', to_frame='world')\n\n random_position = RigidTransform(rotation=np.array([\n [0.9323473, -0.35858258, 0.04612846],\n [-0.35996283, -0.93259467, 0.02597504],\n [0.03370496, -0.04082229, -0.99859775]\n ]), translation=np.array([0.39247965, -0.21613652, 0.3882055]),\n from_frame='franka_tool', to_frame='world')\n\n print(fa.get_pose().translation)\n\n print(fa.get_joints())\n\n desired_joints_1 = [-0.52733715, 0.25603565, 0.47721503, -1.26705864, 0.00600359, 1.60788199, 0.63019184]\n\n desired_joints_2 = [-0.16017485, 1.12476619, 0.26004398, -0.67246923, 0.04899213, 2.08439578, 0.81627789]\n\n fa.reset_joints()\n \n print('Opening Grippers')\n fa.open_gripper()\n\n #fa.reset_pose()\n\n # fa.goto_pose(random_position, use_impedance=False, cartesian_impedances=[3000, 3000, 100, 300, 300, 300])\n\n fa.goto_joints(desired_joints_1, joint_impedances=[100, 100, 100, 50, 50, 100, 100])\n\n fa.goto_joints(desired_joints_2, joint_impedances=[100, 100, 100, 50, 50, 100, 100])\n\n #fa.apply_effector_forces_torques(10.0, 0, 0, 0)" }, { "path": "examples/execute_joint_torques.py", "content": "import argparse\nfrom frankapy import FrankaArm\n\nif __name__ == '__main__':\n parser = argparse.ArgumentParser()\n parser.add_argument('--time', '-t', type=float, default=10)\n args = parser.parse_args()\n\n print('Starting robot')\n fa = FrankaArm()\n fa.reset_joints()\n print('Applying 0 joint torque control for {}s'.format(args.time))\n fa.execute_joint_torques([0,0,0,0,0,3,0], selection=[0,0,0,0,0,1,0], remove_gravity=[0,0,0,0,0,1,0], duration=args.time)" }, { "path": "examples/go_to_joints_with_joint_impedances.py", "content": "import argparse\nimport sys\nfrom frankapy import FrankaArm\nfrom frankapy import FrankaConstants as FC\n\ndef wait_for_enter():\n if sys.version_info[0] < 3:\n raw_input('Press Enter to continue:')\n else:\n input('Press Enter to continue:')\n\nif __name__ == '__main__':\n parser = argparse.ArgumentParser()\n parser.add_argument('--time', '-t', type=float, default=10)\n parser.add_argument('--open_gripper', '-o', action='store_true')\n args = parser.parse_args()\n\n print('Starting robot')\n fa = FrankaArm()\n if args.open_gripper:\n fa.open_gripper()\n\n print('Be very careful!! Make sure the robot can safely move to HOME JOINTS Position.')\n wait_for_enter()\n\n fa.reset_joints()\n print('Using default joint impedances to move back and forth.')\n wait_for_enter()\n fa.goto_joints(FC.READY_JOINTS, joint_impedances=FC.DEFAULT_JOINT_IMPEDANCES)\n fa.goto_joints(FC.HOME_JOINTS)\n print('Now using different joint impedances to move back and forth.')\n wait_for_enter()\n fa.goto_joints(FC.READY_JOINTS, joint_impedances=[1500, 1500, 1500, 1250, 1250, 1000, 1000])\n fa.goto_joints(FC.HOME_JOINTS)\n print('Remember to reset the joint_impedances to defaults.')\n fa.goto_joints(FC.HOME_JOINTS, joint_impedances=FC.DEFAULT_JOINT_IMPEDANCES)\n \n" }, { "path": "examples/move_robot.py", "content": "import numpy as np\nfrom autolab_core import RigidTransform\n\nfrom frankapy import FrankaArm\n\n\nif __name__ == \"__main__\":\n fa = FrankaArm()\n \n # reset franka to its home joints\n fa.reset_joints()\n\n # read functions\n T_ee_world = fa.get_pose()\n print('Translation: {} | Rotation: {}'.format(T_ee_world.translation, T_ee_world.quaternion))\n\n joints = fa.get_joints()\n print('Joints: {}'.format(joints))\n\n gripper_width = fa.get_gripper_width()\n print('Gripper width: {}'.format(gripper_width))\n\n # gripper controls\n print('Closing gripper')\n fa.close_gripper()\n\n print('Opening gripper to a specified position')\n fa.goto_gripper(0.02)\n\n print('Opening gripper all the way')\n fa.open_gripper()\n\n # joint controls\n print('Rotating last joint')\n joints = fa.get_joints()\n joints[6] += np.deg2rad(45)\n fa.goto_joints(joints)\n joints[6] -= np.deg2rad(45)\n fa.goto_joints(joints)\n\n # end-effector pose control\n print('Translation')\n T_ee_world = fa.get_pose()\n T_ee_world.translation += [0.1, 0, 0.1]\n fa.goto_pose(T_ee_world)\n T_ee_world.translation -= [0.1, 0, 0.1]\n fa.goto_pose(T_ee_world)\n\n print('Rotation in end-effector frame')\n T_ee_rot = RigidTransform(\n rotation=RigidTransform.x_axis_rotation(np.deg2rad(45)),\n from_frame='franka_tool', to_frame='franka_tool'\n )\n T_ee_world_target = T_ee_world * T_ee_rot\n fa.goto_pose(T_ee_world_target)\n fa.goto_pose(T_ee_world)\n\n # reset franka back to home\n fa.reset_joints()" }, { "path": "examples/record_trajectory.py", "content": "import argparse\nimport time\nfrom frankapy import FrankaArm\nimport pickle as pkl\n\nif __name__ == '__main__':\n parser = argparse.ArgumentParser()\n parser.add_argument('--time', '-t', type=float, default=10)\n parser.add_argument('--open_gripper', '-o', action='store_true')\n parser.add_argument('--file', '-f', default='franka_traj.pkl')\n args = parser.parse_args()\n\n print('Starting robot')\n fa = FrankaArm()\n if args.open_gripper:\n fa.open_gripper()\n print('Applying 0 force torque control for {}s'.format(args.time))\n end_effector_position = []\n fa.run_guide_mode(args.time, block=False)\n\n for i in range(1000):\n end_effector_position.append(fa.get_pose())\n time.sleep(0.01)\n\n pkl.dump(end_effector_position, open(args.file, 'wb'))" }, { "path": "examples/run_dynamic_cartesian_velocities.py", "content": "import numpy as np\nimport time\n\nfrom frankapy import FrankaConstants as FC\nfrom frankapy import FrankaArm, SensorDataMessageType\nfrom frankapy.proto_utils import sensor_proto2ros_msg, make_sensor_group_msg\nfrom frankapy.proto import CartesianVelocitySensorMessage, ShouldTerminateSensorMessage\nfrom franka_interface_msgs.msg import SensorDataGroup\n\nfrom frankapy.utils import min_jerk\nimport rospy\n\n\nif __name__ == \"__main__\":\n fa = FrankaArm()\n fa.reset_joints()\n\n rospy.loginfo('Generating Trajectory')\n p = fa.get_pose()\n p.translation[2] -= 0.2\n fa.goto_pose(p, duration=5, block=False)\n\n cartesian_accelerations = [1, 1, 1, 0.1, 0.1, 0.1]\n\n T = 5\n dt = 0.01\n ts = np.arange(0, T, dt)\n cartesian_velocities = []\n\n for i in range(len(ts)):\n cartesian_velocities.append(fa.get_robot_state()['cartesian_velocities'])\n time.sleep(dt)\n\n fa.wait_for_skill()\n\n print(cartesian_velocities)\n\n pub = rospy.Publisher(FC.DEFAULT_SENSOR_PUBLISHER_TOPIC, SensorDataGroup, queue_size=1000)\n rate = rospy.Rate(1 / dt)\n\n fa.reset_joints()\n\n rospy.loginfo('Initializing Sensor Publisher')\n\n rospy.loginfo('Publishing cartesian velocity trajectory...')\n # To ensure skill doesn't end before completing trajectory, make the buffer time much longer than needed\n fa.execute_cartesian_velocities(cartesian_velocities[0], cartesian_accelerations, duration=T, dynamic=True, buffer_time=10)\n init_time = rospy.Time.now().to_time()\n for i in range(len(ts)):\n traj_gen_proto_msg = CartesianVelocitySensorMessage(\n id=i, timestamp=rospy.Time.now().to_time() - init_time, \n cartesian_velocities=cartesian_velocities[i]\n )\n ros_msg = make_sensor_group_msg(\n trajectory_generator_sensor_msg=sensor_proto2ros_msg(\n traj_gen_proto_msg, SensorDataMessageType.CARTESIAN_VELOCITY)\n )\n \n rospy.loginfo('Publishing: ID {}'.format(traj_gen_proto_msg.id))\n pub.publish(ros_msg)\n time.sleep(dt)\n\n # Stop the skill\n # Alternatively can call fa.stop_skill()\n term_proto_msg = ShouldTerminateSensorMessage(timestamp=rospy.Time.now().to_time() - init_time, should_terminate=True)\n ros_msg = make_sensor_group_msg(\n termination_handler_sensor_msg=sensor_proto2ros_msg(\n term_proto_msg, SensorDataMessageType.SHOULD_TERMINATE)\n )\n pub.publish(ros_msg)\n\n rospy.loginfo('Done')\n" }, { "path": "examples/run_dynamic_joint_velocities.py", "content": "import numpy as np\nimport time\n\nfrom frankapy import FrankaConstants as FC\nfrom frankapy import FrankaArm, SensorDataMessageType\nfrom frankapy.proto_utils import sensor_proto2ros_msg, make_sensor_group_msg\nfrom frankapy.proto import JointVelocitySensorMessage, ShouldTerminateSensorMessage\nfrom franka_interface_msgs.msg import SensorDataGroup\n\nfrom frankapy.utils import min_jerk\nimport rospy\n\nif __name__ == \"__main__\":\n fa = FrankaArm()\n fa.reset_joints()\n\n rospy.loginfo('Generating Trajectory')\n p = fa.get_pose()\n p.translation[2] -= 0.2\n fa.goto_pose(p, duration=5, block=False)\n\n joint_accelerations = [1, 1, 1, 1, 1, 1, 1]\n\n T = 5\n dt = 0.01\n ts = np.arange(0, T, dt)\n joint_velocities = []\n\n for i in range(len(ts)):\n joint_velocities.append(fa.get_robot_state()['joint_velocities'])\n time.sleep(dt)\n\n fa.wait_for_skill()\n\n pub = rospy.Publisher(FC.DEFAULT_SENSOR_PUBLISHER_TOPIC, SensorDataGroup, queue_size=1000)\n rate = rospy.Rate(1 / dt)\n\n fa.reset_joints()\n\n rospy.loginfo('Initializing Sensor Publisher')\n\n rospy.loginfo('Publishing joints trajectory...')\n # To ensure skill doesn't end before completing trajectory, make the buffer time much longer than needed\n fa.execute_joint_velocities(joint_velocities[0], joint_accelerations, duration=T, dynamic=True, buffer_time=10)\n init_time = rospy.Time.now().to_time()\n for i in range(len(ts)):\n traj_gen_proto_msg = JointVelocitySensorMessage(\n id=i, timestamp=rospy.Time.now().to_time() - init_time, \n joint_velocities=joint_velocities[i]\n )\n ros_msg = make_sensor_group_msg(\n trajectory_generator_sensor_msg=sensor_proto2ros_msg(\n traj_gen_proto_msg, SensorDataMessageType.JOINT_VELOCITY)\n )\n \n rospy.loginfo('Publishing: ID {}'.format(traj_gen_proto_msg.id))\n pub.publish(ros_msg)\n time.sleep(dt)\n\n # Stop the skill\n # Alternatively can call fa.stop_skill()\n term_proto_msg = ShouldTerminateSensorMessage(timestamp=rospy.Time.now().to_time() - init_time, should_terminate=True)\n ros_msg = make_sensor_group_msg(\n termination_handler_sensor_msg=sensor_proto2ros_msg(\n term_proto_msg, SensorDataMessageType.SHOULD_TERMINATE)\n )\n pub.publish(ros_msg)\n\n rospy.loginfo('Done')\n" }, { "path": "examples/run_dynamic_joints.py", "content": "import numpy as np\n\nfrom frankapy import FrankaArm, SensorDataMessageType\nfrom frankapy import FrankaConstants as FC\nfrom frankapy.proto_utils import sensor_proto2ros_msg, make_sensor_group_msg\nfrom frankapy.proto import JointPositionSensorMessage, ShouldTerminateSensorMessage\nfrom franka_interface_msgs.msg import SensorDataGroup\n\nfrom frankapy.utils import min_jerk\n\nimport rospy\n\n\nif __name__ == \"__main__\":\n fa = FrankaArm()\n fa.reset_joints()\n\n rospy.loginfo('Generating Trajectory')\n joints_0 = fa.get_joints()\n p = fa.get_pose()\n p.translation[2] -= 0.2\n fa.goto_pose(p)\n joints_1 = fa.get_joints()\n\n T = 5\n dt = 0.02\n ts = np.arange(0, T, dt)\n joints_traj = [min_jerk(joints_1, joints_0, t, T) for t in ts]\n\n rospy.loginfo('Initializing Sensor Publisher')\n pub = rospy.Publisher(FC.DEFAULT_SENSOR_PUBLISHER_TOPIC, SensorDataGroup, queue_size=1000)\n rate = rospy.Rate(1 / dt)\n\n rospy.loginfo('Publishing joints trajectory...')\n # To ensure skill doesn't end before completing trajectory, make the buffer time much longer than needed\n fa.goto_joints(joints_traj[1], duration=T, dynamic=True, buffer_time=10)\n init_time = rospy.Time.now().to_time()\n for i in range(2, len(ts)):\n traj_gen_proto_msg = JointPositionSensorMessage(\n id=i, timestamp=rospy.Time.now().to_time() - init_time, \n joints=joints_traj[i]\n )\n ros_msg = make_sensor_group_msg(\n trajectory_generator_sensor_msg=sensor_proto2ros_msg(\n traj_gen_proto_msg, SensorDataMessageType.JOINT_POSITION)\n )\n \n rospy.loginfo('Publishing: ID {}'.format(traj_gen_proto_msg.id))\n pub.publish(ros_msg)\n rate.sleep()\n\n # Stop the skill\n # Alternatively can call fa.stop_skill()\n term_proto_msg = ShouldTerminateSensorMessage(timestamp=rospy.Time.now().to_time() - init_time, should_terminate=True)\n ros_msg = make_sensor_group_msg(\n termination_handler_sensor_msg=sensor_proto2ros_msg(\n term_proto_msg, SensorDataMessageType.SHOULD_TERMINATE)\n )\n pub.publish(ros_msg)\n\n rospy.loginfo('Done')\n" }, { "path": "examples/run_dynamic_pose.py", "content": "import numpy as np\n\nfrom autolab_core import RigidTransform\nfrom frankapy import FrankaArm, SensorDataMessageType\nfrom frankapy import FrankaConstants as FC\nfrom frankapy.proto_utils import sensor_proto2ros_msg, make_sensor_group_msg\nfrom frankapy.proto import PosePositionSensorMessage, ShouldTerminateSensorMessage, CartesianImpedanceSensorMessage\nfrom franka_interface_msgs.msg import SensorDataGroup\n\nfrom frankapy.utils import min_jerk, min_jerk_weight\n\nimport rospy\n\n\nif __name__ == \"__main__\":\n fa = FrankaArm()\n fa.reset_joints()\n\n rospy.loginfo('Generating Trajectory')\n p0 = fa.get_pose()\n p1 = p0.copy()\n T_delta = RigidTransform(\n translation=np.array([0, 0, 0.2]),\n rotation=RigidTransform.z_axis_rotation(np.deg2rad(30)), \n from_frame=p1.from_frame, to_frame=p1.from_frame)\n p1 = p1 * T_delta\n fa.goto_pose(p1)\n\n T = 5\n dt = 0.02\n ts = np.arange(0, T, dt)\n has_closed = False\n\n weights = [min_jerk_weight(t, T) for t in ts]\n pose_traj = [p1.interpolate_with(p0, w) for w in weights]\n\n z_stiffness_traj = [min_jerk(100, 800, t, T) for t in ts]\n\n rospy.loginfo('Initializing Sensor Publisher')\n pub = rospy.Publisher(FC.DEFAULT_SENSOR_PUBLISHER_TOPIC, SensorDataGroup, queue_size=1000)\n rate = rospy.Rate(1 / dt)\n\n rospy.loginfo('Publishing pose trajectory...')\n # To ensure skill doesn't end before completing trajectory, make the buffer time much longer than needed\n fa.goto_pose(pose_traj[1], duration=T, dynamic=True, buffer_time=10,\n cartesian_impedances=FC.DEFAULT_TRANSLATIONAL_STIFFNESSES[:2] + [z_stiffness_traj[1]] + FC.DEFAULT_ROTATIONAL_STIFFNESSES\n )\n init_time = rospy.Time.now().to_time()\n for i in range(2, len(ts)):\n timestamp = rospy.Time.now().to_time() - init_time\n traj_gen_proto_msg = PosePositionSensorMessage(\n id=i, timestamp=timestamp, \n position=pose_traj[i].translation, quaternion=pose_traj[i].quaternion\n )\n fb_ctrlr_proto = CartesianImpedanceSensorMessage(\n id=i, timestamp=timestamp,\n translational_stiffnesses=FC.DEFAULT_TRANSLATIONAL_STIFFNESSES[:2] + [z_stiffness_traj[i]],\n rotational_stiffnesses=FC.DEFAULT_ROTATIONAL_STIFFNESSES\n )\n ros_msg = make_sensor_group_msg(\n trajectory_generator_sensor_msg=sensor_proto2ros_msg(\n traj_gen_proto_msg, SensorDataMessageType.POSE_POSITION),\n feedback_controller_sensor_msg=sensor_proto2ros_msg(\n fb_ctrlr_proto, SensorDataMessageType.CARTESIAN_IMPEDANCE)\n )\n \n rospy.loginfo('Publishing: ID {}'.format(traj_gen_proto_msg.id))\n pub.publish(ros_msg)\n rate.sleep()\n\n # Stop the skill\n # Alternatively can call fa.stop_skill()\n term_proto_msg = ShouldTerminateSensorMessage(timestamp=rospy.Time.now().to_time() - init_time, should_terminate=True)\n ros_msg = make_sensor_group_msg(\n termination_handler_sensor_msg=sensor_proto2ros_msg(\n term_proto_msg, SensorDataMessageType.SHOULD_TERMINATE)\n )\n pub.publish(ros_msg)\n\n rospy.loginfo('Done')\n" }, { "path": "examples/run_hfpc.py", "content": "import numpy as np\n\nfrom frankapy import FrankaArm, SensorDataMessageType\nfrom frankapy import FrankaConstants as FC\nfrom frankapy.proto_utils import sensor_proto2ros_msg, make_sensor_group_msg\nfrom frankapy.proto import ForcePositionSensorMessage, ForcePositionControllerSensorMessage\nfrom franka_interface_msgs.msg import SensorDataGroup\nfrom frankapy.utils import transform_to_list, min_jerk\n\nfrom tqdm import trange\n\nimport rospy\n\n\nif __name__ == \"__main__\":\n fa = FrankaArm()\n fa.reset_joints()\n fa.close_gripper()\n\n while True:\n input('Presse [Enter] to enter guide mode and move robot to be on top of a flat surface.')\n fa.run_guide_mode()\n while True:\n inp = input('[r]etry or [c]ontinue? ')\n if inp not in ('r', 'c'):\n print('Please give valid input!')\n else:\n break\n if inp == 'c':\n break\n\n rospy.loginfo('Generating Trajectory')\n # EE will follow a 2D circle while pressing down with a target force\n dt = 0.01\n T = 10\n ts = np.arange(0, T, dt)\n N = len(ts)\n dthetas = np.linspace(-np.pi / 2, 3 * np.pi / 2, N)\n r = 0.07\n circ = r * np.c_[np.sin(dthetas), np.cos(dthetas)]\n\n start_pose = fa.get_pose()\n start_pose.rotation = FC.HOME_POSE.rotation\n target_poses = []\n for i, t in enumerate(ts):\n pose = start_pose.copy()\n pose.translation[0] += r + circ[i, 0]\n pose.translation[1] += circ[i, 1]\n target_poses.append(pose)\n target_force = [0, 0, -10, 0, 0, 0]\n S = [1, 1, 1, 1, 1, 1]\n position_kps_cart = FC.DEFAULT_TRANSLATIONAL_STIFFNESSES + FC.DEFAULT_ROTATIONAL_STIFFNESSES\n force_kps_cart = [0.1] * 6\n position_kps_joint = FC.DEFAULT_K_GAINS\n force_kps_joint = [0.1] * 7\n\n rospy.loginfo('Initializing Sensor Publisher')\n pub = rospy.Publisher(FC.DEFAULT_SENSOR_PUBLISHER_TOPIC, SensorDataGroup, queue_size=1)\n rate = rospy.Rate(1 / dt)\n n_times = 1\n\n rospy.loginfo('Publishing HFPC trajectory w/ cartesian gains...')\n fa.run_dynamic_force_position(duration=T * n_times, buffer_time=3, S=S,\n use_cartesian_gains=True,\n position_kps_cart=position_kps_cart,\n force_kps_cart=force_kps_cart)\n init_time = rospy.Time.now().to_time()\n for i in trange(N * n_times):\n t = i % N\n timestamp = rospy.Time.now().to_time() - init_time\n traj_gen_proto_msg = ForcePositionSensorMessage(\n id=i, timestamp=timestamp, seg_run_time=dt,\n pose=transform_to_list(target_poses[t]),\n force=target_force\n )\n fb_ctrlr_proto = ForcePositionControllerSensorMessage(\n id=i, timestamp=timestamp,\n position_kps_cart=position_kps_cart,\n force_kps_cart=force_kps_cart,\n selection=S\n )\n ros_msg = make_sensor_group_msg(\n trajectory_generator_sensor_msg=sensor_proto2ros_msg(\n traj_gen_proto_msg, SensorDataMessageType.FORCE_POSITION),\n feedback_controller_sensor_msg=sensor_proto2ros_msg(\n fb_ctrlr_proto, SensorDataMessageType.FORCE_POSITION_GAINS)\n )\n pub.publish(ros_msg)\n rate.sleep()\n fa.stop_skill()\n\n rospy.loginfo('Publishing HFPC trajectory w/ joint gains...')\n fa.run_dynamic_force_position(duration=T * n_times, buffer_time=3, S=S,\n use_cartesian_gains=False,\n position_kps_joint=position_kps_joint,\n force_kps_joint=force_kps_joint)\n init_time = rospy.Time.now().to_time()\n for i in trange(N * n_times):\n t = i % N\n timestamp = rospy.Time.now().to_time() - init_time\n traj_gen_proto_msg = ForcePositionSensorMessage(\n id=i, timestamp=timestamp, seg_run_time=dt,\n pose=transform_to_list(target_poses[t]),\n force=target_force\n )\n fb_ctrlr_proto = ForcePositionControllerSensorMessage(\n id=i, timestamp=timestamp,\n position_kps_joint=position_kps_joint,\n force_kps_joint=force_kps_joint,\n selection=S\n )\n ros_msg = make_sensor_group_msg(\n trajectory_generator_sensor_msg=sensor_proto2ros_msg(\n traj_gen_proto_msg, SensorDataMessageType.FORCE_POSITION),\n feedback_controller_sensor_msg=sensor_proto2ros_msg(\n fb_ctrlr_proto, SensorDataMessageType.FORCE_POSITION_GAINS)\n )\n pub.publish(ros_msg)\n rate.sleep()\n fa.stop_skill()\n\n fa.reset_joints()\n fa.open_gripper()\n \n rospy.loginfo('Done')\n" }, { "path": "examples/run_pose_dmp.py", "content": "import numpy as np\nimport math\nimport rospy\nimport argparse\nimport pickle\nfrom autolab_core import RigidTransform, Point\nfrom frankapy import FrankaArm\n\nif __name__ == '__main__':\n parser = argparse.ArgumentParser()\n parser.add_argument('--pose_dmp_weights_file_path', '-f', type=str)\n args = parser.parse_args()\n\n print('Starting robot')\n fa = FrankaArm();\n\n with open(args.pose_dmp_weights_file_path, 'rb') as pkl_f:\n pose_dmp_info = pickle.load(pkl_f)\n fa.execute_pose_dmp(pose_dmp_info, duration=8)" }, { "path": "examples/run_quaternion_pose_dmp.py", "content": "import numpy as np\nimport math\nimport rospy\nimport argparse\nimport pickle\nfrom autolab_core import RigidTransform, Point\nfrom frankapy import FrankaArm\n\ndef execute_quaternion_pose_dmp(fa, position_dmp_weights_path, quat_dmp_weights_path, \n goal_quat=(0.03, 1.0, -0.03, 0.01),\n duration=10.0):\n position_dmp_file = open(position_dmp_weights_path, 'rb')\n position_dmp_info = pickle.load(position_dmp_file)\n\n quat_dmp_file = open(quat_dmp_weights_path, 'rb')\n quat_dmp_info = pickle.load(quat_dmp_file)\n\n # Should be less than duration so that the canonical system is set to 0 appropriately\n quat_goal_time = duration - 3.0\n fa.execute_quaternion_pose_dmp(position_dmp_info, quat_dmp_info, duration, goal_quat, quat_goal_time)\n\nif __name__ == '__main__':\n parser = argparse.ArgumentParser()\n parser.add_argument('--position_dmp_weights_path', '-p', type=str)\n parser.add_argument('--quat_dmp_weights_path', '-q', type=str)\n args = parser.parse_args()\n\n print('Starting robot')\n fa = FrankaArm();\n\n execute_quaternion_pose_dmp(fa, \n args.position_dmp_weights_path, \n args.quat_dmp_weights_path, \n goal_quat=(0.03, 1.0, -0.03, 0.01),\n duration=20.0):\n" }, { "path": "examples/run_recorded_trajectory.py", "content": "import pickle as pkl\nimport numpy as np\n\nfrom autolab_core import RigidTransform\nfrom frankapy import FrankaArm, SensorDataMessageType\nfrom frankapy import FrankaConstants as FC\nfrom frankapy.proto_utils import sensor_proto2ros_msg, make_sensor_group_msg\nfrom frankapy.proto import PosePositionSensorMessage, ShouldTerminateSensorMessage, CartesianImpedanceSensorMessage\nfrom franka_interface_msgs.msg import SensorDataGroup\n\nfrom frankapy.utils import min_jerk, min_jerk_weight\n\nimport rospy\n\nif __name__ == \"__main__\":\n fa = FrankaArm()\n fa.reset_joints()\n\n rospy.loginfo('Generating Trajectory')\n\n pose_traj = pkl.load(open('franka_traj.pkl','rb'))\n\n T = 10\n dt = 0.01\n ts = np.arange(0, T, dt)\n\n rospy.loginfo('Initializing Sensor Publisher')\n pub = rospy.Publisher(FC.DEFAULT_SENSOR_PUBLISHER_TOPIC, SensorDataGroup, queue_size=10)\n rate = rospy.Rate(1 / dt)\n\n rospy.loginfo('Publishing pose trajectory...')\n # To ensure skill doesn't end before completing trajectory, make the buffer time much longer than needed\n fa.goto_pose(pose_traj[1], duration=T, dynamic=True, buffer_time=10,\n cartesian_impedances=[600.0, 600.0, 600.0, 50.0, 50.0, 50.0]\n )\n init_time = rospy.Time.now().to_time()\n for i in range(2, len(ts)):\n timestamp = rospy.Time.now().to_time() - init_time\n traj_gen_proto_msg = PosePositionSensorMessage(\n id=i, timestamp=timestamp,\n position=pose_traj[i].translation, quaternion=pose_traj[i].quaternion\n\t\t)\n ros_msg = make_sensor_group_msg(\n trajectory_generator_sensor_msg=sensor_proto2ros_msg(\n traj_gen_proto_msg, SensorDataMessageType.POSE_POSITION),\n )\n\n rospy.loginfo('Publishing: ID {}'.format(traj_gen_proto_msg.id))\n pub.publish(ros_msg)\n rate.sleep()\n\n # Stop the skill\n # Alternatively can call fa.stop_skill()\n term_proto_msg = ShouldTerminateSensorMessage(timestamp=rospy.Time.now().to_time() - init_time, should_terminate=True)\n ros_msg = make_sensor_group_msg(\n termination_handler_sensor_msg=sensor_proto2ros_msg(\n term_proto_msg, SensorDataMessageType.SHOULD_TERMINATE)\n )\n pub.publish(ros_msg)\n\n rospy.loginfo('Done')\n" }, { "path": "frankapy/__init__.py", "content": "from .franka_arm import FrankaArm\nfrom .franka_constants import FrankaConstants\nfrom .franka_arm_state_client import FrankaArmStateClient\nfrom .exceptions import FrankaArmCommException\nfrom .franka_interface_common_definitions import SkillType, MetaSkillType, TrajectoryGeneratorType, FeedbackControllerType, TerminationHandlerType, SkillStatus, SensorDataMessageType" }, { "path": "frankapy/exceptions.py", "content": "class FrankaArmCommException(Exception):\n ''' Communication failure. Usually occurs due to timeouts.\n '''\n def __init__(self, message, *args, **kwargs):\n Exception.__init__(self, *args, **kwargs)\n self.message = message\n \n def __str__(self):\n return \"Communication w/ FrankaInterface ran into a problem: {}. FrankaInterface is probably not ready.\".format(self.message)\n\n\nclass FrankaArmFrankaInterfaceNotReadyException(Exception):\n ''' Exception for when franka_interface is not ready\n '''\n\n def __init__(self, *args, **kwargs):\n Exception.__init__(self, *args, **kwargs)\n\n def __str__(self):\n return 'FrankaInterface was not ready!'\n\n\nclass FrankaArmException(Exception):\n ''' Failure of control, typically due to a kinematically unreachable pose.\n '''\n\n def __init__(self, message, *args, **kwargs):\n Exception.__init__(self, *args, **kwargs)\n self.message = message\n\n def __str__(self):\n return \"FrankaInterface ran into a problem: {}\".format(self.message)\n" }, { "path": "frankapy/franka_arm.py", "content": "\"\"\"\nfranka_arm.py\n====================================\nThe core module of frankapy\n\"\"\"\n\nimport sys, signal, logging\nfrom time import time, sleep\nimport numpy as np\nfrom autolab_core import RigidTransform\nimport quaternion\nfrom itertools import product\n\nimport roslib\nroslib.load_manifest('franka_interface_msgs')\nimport rospy\nfrom actionlib import SimpleActionClient\nfrom sensor_msgs.msg import JointState\nfrom franka_interface_msgs.msg import ExecuteSkillAction, SensorDataGroup\nfrom franka_interface_msgs.srv import GetCurrentFrankaInterfaceStatusCmd\nfrom franka_gripper.msg import *\n\nfrom .skill_list import *\nfrom .exceptions import *\nfrom .franka_arm_state_client import FrankaArmStateClient\nfrom .franka_constants import FrankaConstants as FC\nfrom .franka_interface_common_definitions import *\nfrom .ros_utils import BoxesPublisher\n\n\nclass FrankaArm:\n\n def __init__(\n self,\n rosnode_name='franka_arm_client', \n ros_log_level=rospy.INFO,\n robot_num=1,\n with_gripper=True,\n old_gripper=False,\n offline=False,\n init_node=True):\n\n \"\"\"\n Initialize a FrankaArm.\n\n Parameters\n ----------\n rosnode_name : :obj:`str`\n A name for the FrankaArm ROS Node.\n\n ros_log_level : :obj:`rospy.log_level` of int\n Passed to rospy.init_node.\n\n robot_num : :obj:`int`\n Number assigned to the current robot.\n\n with_gripper : :obj:`bool`\n Flag for whether the Franka gripper is attached.\n\n old_gripper : :obj:`bool`\n Flag for whether to run the franka_ros gripper or the old_gripper \n implementation.\n\n offline : :obj:`bool`\n Flag for whether to run the robot in the real world.\n\n \"\"\"\n\n self._execute_skill_action_server_name = \\\n '/execute_skill_action_server_node_{}/execute_skill'.format(robot_num)\n self._robot_state_server_name = \\\n '/get_current_robot_state_server_node_{}/get_current_robot_state_server'.format(robot_num)\n self._franka_interface_status_server_name = \\\n '/get_current_franka_interface_status_server_node_{}/get_current_franka_interface_status_server'.format(robot_num)\n self._gripper_homing_action_server_name = \\\n '/franka_gripper_{}/homing'.format(robot_num)\n self._gripper_move_action_server_name = \\\n '/franka_gripper_{}/move'.format(robot_num)\n self._gripper_stop_action_server_name = \\\n '/franka_gripper_{}/stop'.format(robot_num)\n self._gripper_grasp_action_server_name = \\\n '/franka_gripper_{}/grasp'.format(robot_num)\n self._gripper_joint_states_name = \\\n '/franka_gripper_{}/joint_states'.format(robot_num)\n if robot_num == 1:\n self._sensor_publisher_name = \\\n '/franka_ros_interface/sensor'\n else:\n self._sensor_publisher_name = \\\n '/franka_ros_interface_{}/sensor'.format(robot_num)\n \n self._connected = False\n self._in_skill = False\n self._offline = offline\n self._with_gripper = with_gripper\n self._old_gripper = old_gripper\n self._last_gripper_command = None\n\n # init ROS\n if init_node:\n rospy.init_node(rosnode_name,\n disable_signals=True,\n log_level=ros_log_level)\n self._collision_boxes_pub = BoxesPublisher('franka_collision_boxes_{}'.format(robot_num))\n self._joint_state_pub = rospy.Publisher('franka_virtual_joints_{}'.format(robot_num), JointState, queue_size=10)\n self._sensor_pub = rospy.Publisher(self._sensor_publisher_name, SensorDataGroup, queue_size=100)\n \n self._state_client = FrankaArmStateClient(\n new_ros_node=False,\n robot_state_server_name=self._robot_state_server_name,\n offline=self._offline)\n\n if not self._offline:\n # set signal handler to handle ctrl+c and kill sigs\n signal.signal(signal.SIGINT, self._sigint_handler_gen())\n \n rospy.wait_for_service(self._franka_interface_status_server_name)\n self._get_current_franka_interface_status = rospy.ServiceProxy(\n self._franka_interface_status_server_name, GetCurrentFrankaInterfaceStatusCmd)\n\n self._client = SimpleActionClient(\n self._execute_skill_action_server_name, ExecuteSkillAction)\n self._client.wait_for_server()\n self.wait_for_franka_interface()\n\n if self._with_gripper and not self._old_gripper:\n self._gripper_homing_client = SimpleActionClient(\n self._gripper_homing_action_server_name, HomingAction)\n self._gripper_homing_client.wait_for_server()\n self._gripper_move_client = SimpleActionClient(\n self._gripper_move_action_server_name, MoveAction)\n self._gripper_move_client.wait_for_server()\n self._gripper_grasp_client = SimpleActionClient(\n self._gripper_grasp_action_server_name, GraspAction)\n self._gripper_grasp_client.wait_for_server()\n self._gripper_stop_client = SimpleActionClient(\n self._gripper_stop_action_server_name, StopAction)\n self._gripper_stop_client.wait_for_server()\n\n # done init ROS\n self._connected = True\n\n # set default identity tool delta pose\n self._tool_delta_pose = RigidTransform(from_frame='franka_tool', \n to_frame='franka_tool_base')\n\n # Precompute things and preallocate np memory for collision checking\n self._collision_boxes_data = np.zeros((len(FC.COLLISION_BOX_SHAPES), 10))\n self._collision_boxes_data[:, -3:] = FC.COLLISION_BOX_SHAPES\n\n self._collision_box_hdiags = []\n self._collision_box_vertices_offset = []\n self._vertex_offset_signs = np.array(list(product([1, -1],[1,-1], [1,-1])))\n for sizes in FC.COLLISION_BOX_SHAPES:\n hsizes = sizes/2\n self._collision_box_vertices_offset.append(self._vertex_offset_signs * hsizes)\n self._collision_box_hdiags.append(np.linalg.norm(sizes/2))\n self._collision_box_vertices_offset = np.array(self._collision_box_vertices_offset)\n self._collision_box_hdiags = np.array(self._collision_box_hdiags)\n\n self._collision_proj_axes = np.zeros((3, 15))\n self._box_vertices_offset = np.ones([8, 3])\n self._box_transform = np.eye(4)\n\n def wait_for_franka_interface(self, timeout=None):\n \"\"\"\n Blocks execution until franka-interface gives ready signal.\n\n Parameters\n ----------\n timeout : :obj:`float`\n Timeout in seconds to wait for franka-interface.\n \"\"\"\n timeout = FC.DEFAULT_FRANKA_INTERFACE_TIMEOUT if timeout is None else timeout\n t_start = time()\n while time() - t_start < timeout:\n franka_interface_status = self._get_current_franka_interface_status().franka_interface_status\n if franka_interface_status.is_ready:\n return\n sleep(1e-2)\n raise FrankaArmCommException('FrankaInterface status not ready for {}s'.format(\n FC.DEFAULT_FRANKA_INTERFACE_TIMEOUT))\n\n def wait_for_skill(self):\n \"\"\"\n Blocks execution until skill is done.\n \"\"\"\n while not self.is_skill_done():\n continue\n\n def wait_for_gripper(self):\n \"\"\"\n Blocks execution until gripper is done.\n \"\"\"\n if self._last_gripper_command == \"Grasp\":\n done = self._gripper_grasp_client.wait_for_result()\n elif self._last_gripper_command == \"Homing\":\n done = self._gripper_homing_client.wait_for_result()\n elif self._last_gripper_command == \"Stop\":\n done = self._gripper_stop_client.wait_for_result()\n elif self._last_gripper_command == \"Move\":\n done = self._gripper_move_client.wait_for_result()\n sleep(2)\n\n\n def is_skill_done(self, ignore_errors=True): \n \"\"\"\n Checks whether skill is done.\n\n Parameters\n ----------\n ignore_errors : :obj:`bool`\n Flag of whether to ignore errors.\n\n Returns\n -------\n :obj:`bool`\n Flag of whether the skill is done.\n \"\"\" \n if not self._in_skill: \n return True \n\n franka_interface_status = self._get_current_franka_interface_status().franka_interface_status \n\n e = None \n if rospy.is_shutdown(): \n e = RuntimeError('rospy is down!') \n elif franka_interface_status.error_description: \n e = FrankaArmException(franka_interface_status.error_description) \n elif not franka_interface_status.is_ready: \n e = FrankaArmFrankaInterfaceNotReadyException() \n\n if e is not None: \n if ignore_errors: \n self.wait_for_franka_interface() \n else: \n raise e \n\n done = self._client.wait_for_result(rospy.Duration.from_sec( \n FC.ACTION_WAIT_LOOP_TIME))\n\n if done: \n self._in_skill = False \n\n return done\n\n def stop_skill(self): \n \"\"\"\n Stops the current skill.\n \"\"\"\n if self._connected and self._in_skill:\n self._client.cancel_goal()\n self.wait_for_skill()\n\n def _sigint_handler_gen(self):\n def sigint_handler(sig, frame):\n if self._connected and self._in_skill:\n self._client.cancel_goal()\n sys.exit(0)\n\n return sigint_handler\n\n def _send_goal(self, goal, cb, block=True, ignore_errors=True):\n \"\"\"\n Sends the goal to the Franka Interface ROS Action Server\n\n Parameters\n ----------\n\n goal : :obj:`ExecuteSkillGoal`\n Skill with parameters that are sent to franka-interface.\n\n cb : :obj:`function`\n Function that takes in ExecuteSkillFeedback during skill execution.\n\n block : :obj:`bool`\n Flag that determines whether to block until skill is finished.\n\n ignore_errors : :obj:`bool`\n Flag of whether to ignore errors.\n\n Returns\n -------\n :obj:`ExecuteSkillResult`\n Skill Results that are sent from franka-interface.\n\n Raises\n ------\n FrankaArmCommException if a timeout is reached\n FrankaArmException if franka-interface gives an error\n FrankaArmFrankaInterfaceNotReadyException if franka-interface is not ready\n \"\"\"\n if self._offline:\n logging.warn('In offline mode, FrankaArm will not execute real robot commands.')\n return\n\n if not self.is_skill_done(): \n raise ValueError('Cannot send another command when the previous skill is active!')\n\n self._in_skill = True\n self._client.send_goal(goal, feedback_cb=cb)\n\n if not block: \n return None\n\n self.wait_for_skill()\n return self._client.get_result()\n\n \"\"\"\n Controls\n \"\"\"\n\n def goto_pose(self,\n tool_pose,\n duration=3,\n use_impedance=True,\n dynamic=False,\n buffer_time=FC.DEFAULT_TERM_BUFFER_TIME,\n force_thresholds=None,\n torque_thresholds=None,\n cartesian_impedances=None,\n joint_impedances=None,\n block=True,\n ignore_errors=True,\n ignore_virtual_walls=False,\n skill_desc='GoToPose'):\n \"\"\"\n Commands Arm to the given pose via min jerk interpolation\n\n Parameters\n ----------\n tool_pose : :obj:`autolab_core.RigidTransform`\n End-effector pose in tool frame\n duration : :obj:`float`\n How much time this robot motion should take.\n use_impedance : :obj:`bool`\n Function uses our impedance controller by default. \n If False, uses the Franka cartesian controller.\n dynamic : :obj:`bool`\n Flag that states whether the skill is dynamic. \n If True, it will use our joint impedance controller \n and sensor values.\n buffer_time : :obj:`float`\n How much extra time the termination handler will wait\n before stopping the skill after duration has passed.\n force_thresholds : :obj:`list`\n List of 6 floats corresponding to force limits on \n translation (xyz) and rotation about (xyz) axes.\n Default is None. If None then will not stop on contact.\n torque_thresholds : :obj:`list`\n List of 7 floats corresponding to torque limits on each joint. \n Default is None. If None then will not stop on contact.\n cartesian_impedances : :obj:`list`\n List of 6 floats corresponding to impedances on translation \n (xyz) and rotation about (xyz) axes. Default is None. \n If None then will use default impedances.\n joint_impedances : :obj:`list` \n List of 7 floats corresponding to impedances on each joint. \n This is used when use_impedance is False. Default is None. \n If None then will use default impedances.\n block : :obj:`bool` \n Function blocks by default. If False, the function becomes\n asynchronous and can be preempted.\n ignore_errors : :obj:`bool` \n Function ignores errors by default. If False, errors and \n some exceptions can be thrown.\n ignore_virtual_walls : :obj:`bool` \n Function checks for collisions with virtual walls by default. \n If False, the robot no longer checks, which may be dangerous.\n skill_desc : :obj:`str` \n Skill description to use for logging on the Control PC.\n\n Raises\n ------\n ValueError: If tool_pose does not have from_frame=franka_tool and \n to_frame=world or tool_pose is outside the virtual walls.\n \"\"\"\n if dynamic:\n skill = Skill(SkillType.ImpedanceControlSkill, \n TrajectoryGeneratorType.PassThroughPoseTrajectoryGenerator,\n feedback_controller_type=FeedbackControllerType.CartesianImpedanceFeedbackController,\n termination_handler_type=TerminationHandlerType.TimeTerminationHandler, \n skill_desc=skill_desc)\n use_impedance=True\n block = False\n else:\n if use_impedance:\n skill = Skill(SkillType.ImpedanceControlSkill, \n TrajectoryGeneratorType.MinJerkPoseTrajectoryGenerator,\n feedback_controller_type=FeedbackControllerType.CartesianImpedanceFeedbackController,\n termination_handler_type=TerminationHandlerType.FinalPoseTerminationHandler, \n skill_desc=skill_desc)\n else:\n skill = Skill(SkillType.CartesianPoseSkill, \n TrajectoryGeneratorType.MinJerkPoseTrajectoryGenerator,\n feedback_controller_type=FeedbackControllerType.SetInternalImpedanceFeedbackController,\n termination_handler_type=TerminationHandlerType.FinalPoseTerminationHandler, \n skill_desc=skill_desc)\n \n if tool_pose.from_frame != 'franka_tool' or tool_pose.to_frame != 'world':\n raise ValueError('pose has invalid frame names! Make sure pose has \\\n from_frame=franka_tool and to_frame=world')\n\n tool_base_pose = tool_pose * self._tool_delta_pose.inverse()\n\n if not ignore_virtual_walls and np.any([\n tool_base_pose.translation <= FC.WORKSPACE_WALLS[:, :3].min(axis=0),\n tool_base_pose.translation >= FC.WORKSPACE_WALLS[:, :3].max(axis=0)]):\n raise ValueError('Target pose is outside of workspace virtual walls!')\n\n skill.add_initial_sensor_values(FC.EMPTY_SENSOR_VALUES)\n\n skill.set_cartesian_impedances(use_impedance, cartesian_impedances, joint_impedances)\n\n if not skill.check_for_contact_params(buffer_time, force_thresholds, torque_thresholds):\n if dynamic:\n skill.add_time_termination_params(buffer_time)\n else:\n skill.add_pose_threshold_params(buffer_time, FC.DEFAULT_POSE_THRESHOLDS)\n\n skill.add_goal_pose(duration, tool_base_pose)\n goal = skill.create_goal()\n\n self._send_goal(goal,\n cb=lambda x: skill.feedback_callback(x),\n block=block,\n ignore_errors=ignore_errors)\n\n if dynamic:\n sleep(FC.DYNAMIC_SKILL_WAIT_TIME)\n\n def goto_pose_delta(self,\n delta_tool_pose,\n duration=3,\n use_impedance=True,\n buffer_time=FC.DEFAULT_TERM_BUFFER_TIME,\n force_thresholds=None,\n torque_thresholds=None,\n cartesian_impedances=None,\n joint_impedances=None,\n block=True,\n ignore_errors=True,\n ignore_virtual_walls=False,\n skill_desc='GoToPoseDelta'):\n \"\"\"\n Commands Arm to the given delta pose via min jerk interpolation\n\n Parameters\n ----------\n delta_tool_pose : :obj:`autolab_core.RigidTransform` \n Delta pose in tool frame\n duration : :obj:`float` \n How much time this robot motion should take.\n use_impedance : :obj:`bool` \n Function uses our impedance controller by default. \n If False, uses the Franka cartesian controller.\n buffer_time : :obj:`float` \n How much extra time the termination handler will wait\n before stopping the skill after duration has passed.\n force_thresholds : :obj:`list` \n List of 6 floats corresponding to force limits on translation \n (xyz) and rotation about (xyz) axes.\n Default is None. If None then will not stop on contact.\n torque_thresholds : :obj:`list` \n List of 7 floats corresponding to torque limits on each joint. \n Default is None. If None then will not stop on contact.\n cartesian_impedances : :obj:`list` \n List of 6 floats corresponding to impedances on translation \n (xyz) and rotation about (xyz) axes. Default is None. \n If None then will use default impedances.\n joint_impedances : :obj:`list` \n List of 7 floats corresponding to impedances on each joint. \n This is used when use_impedance is False. Default is None. \n If None then will use default impedances.\n block : :obj:`bool` \n Function blocks by default. If False, the function becomes\n asynchronous and can be preempted.\n ignore_errors : :obj:`bool` \n Function ignores errors by default. If False, errors and \n some exceptions can be thrown.\n ignore_virtual_walls : :obj:`bool` \n Function checks for collisions with virtual walls by default. \n If False, the robot no longer checks, which may be dangerous.\n skill_desc : :obj:`str` \n Skill description to use for logging on the Control PC.\n\n Raises:\n ValueError: If delta_pose does not have from_frame=franka_tool and \n to_frame=franka_tool or from_frame=world and to_frame=world.\n \"\"\"\n if use_impedance:\n skill = Skill(SkillType.ImpedanceControlSkill, \n TrajectoryGeneratorType.RelativeMinJerkPoseTrajectoryGenerator,\n feedback_controller_type=FeedbackControllerType.CartesianImpedanceFeedbackController,\n termination_handler_type=TerminationHandlerType.FinalPoseTerminationHandler, \n skill_desc=skill_desc)\n else:\n skill = Skill(SkillType.CartesianPoseSkill, \n TrajectoryGeneratorType.RelativeMinJerkPoseTrajectoryGenerator,\n feedback_controller_type=FeedbackControllerType.SetInternalImpedanceFeedbackController,\n termination_handler_type=TerminationHandlerType.FinalPoseTerminationHandler, \n skill_desc=skill_desc)\n\n skill.add_initial_sensor_values(FC.EMPTY_SENSOR_VALUES)\n\n skill.set_cartesian_impedances(use_impedance, cartesian_impedances, joint_impedances)\n\n if not skill.check_for_contact_params(buffer_time, force_thresholds, torque_thresholds):\n skill.add_pose_threshold_params(buffer_time, FC.DEFAULT_POSE_THRESHOLDS)\n\n if delta_tool_pose.from_frame == 'world' \\\n and delta_tool_pose.to_frame == 'world':\n\n skill.add_goal_pose(duration, delta_tool_pose)\n\n elif delta_tool_pose.from_frame == 'franka_tool' \\\n and delta_tool_pose.to_frame == 'franka_tool':\n starting_pose = self.get_pose()\n\n starting_tool_base_pose = starting_pose * self._tool_delta_pose.inverse()\n\n final_goal_pose = starting_pose * delta_tool_pose\n\n final_tool_base_pose = final_goal_pose * self._tool_delta_pose.inverse()\n\n delta_tool_base_pose = starting_tool_base_pose.inverse() * final_tool_base_pose\n\n starting_rotation = RigidTransform(\n rotation=starting_pose.rotation,\n from_frame='franka_tool', to_frame='franka_tool'\n )\n predicted_translation = RigidTransform(\n translation=delta_tool_base_pose.translation,\n from_frame='franka_tool', to_frame='franka_tool'\n )\n actual_translation = starting_rotation * predicted_translation\n delta_tool_base_pose.translation = actual_translation.translation\n\n if not ignore_virtual_walls and not self._offline:\n if np.any([\n final_tool_base_pose.translation <= FC.WORKSPACE_WALLS[:, :3].min(axis=0),\n final_tool_base_pose.translation >= FC.WORKSPACE_WALLS[:, :3].max(axis=0)]):\n raise ValueError('Target pose is outside of workspace virtual walls!')\n\n skill.add_goal_pose(duration, delta_tool_base_pose)\n\n else:\n raise ValueError('delta_pose has invalid frame names! ' \\\n 'Make sure delta_pose has from_frame=franka_tool ' \\\n 'and to_frame=franka_tool or from_frame=world and \\\n to_frame=world')\n\n goal = skill.create_goal()\n\n self._send_goal(goal,\n cb=lambda x: skill.feedback_callback(x),\n block=block,\n ignore_errors=ignore_errors)\n \n\n def goto_joints(self,\n joints,\n duration=5,\n use_impedance=False,\n dynamic=False,\n buffer_time=FC.DEFAULT_TERM_BUFFER_TIME,\n force_thresholds=None,\n torque_thresholds=None,\n cartesian_impedances=None,\n joint_impedances=None,\n k_gains=None,\n d_gains=None,\n block=True,\n ignore_errors=True,\n ignore_virtual_walls=False,\n skill_desc='GoToJoints'):\n \"\"\"\n Commands Arm to the given joint configuration\n\n Parameters\n ----------\n joints : :obj:`list` \n A list of 7 numbers that correspond to joint angles in radians.\n duration : :obj:`float` \n How much time this robot motion should take.\n use_impedance : :obj:`bool` \n Function uses the Franka joint impedance controller by default. \n If True, uses our joint impedance controller.\n dynamic : :obj:`bool` \n Flag that states whether the skill is dynamic. If True, it \n will use our joint impedance controller and sensor values.\n buffer_time : :obj:`float` \n How much extra time the termination handler will wait\n before stopping the skill after duration has passed.\n force_thresholds : :obj:`list` \n List of 6 floats corresponding to force limits on translation \n (xyz) and rotation about (xyz) axes. Default is None. \n If None then will not stop on contact.\n torque_thresholds : :obj:`list` \n List of 7 floats corresponding to torque limits on each joint. \n Default is None. If None then will not stop on contact.\n cartesian_impedances : :obj:`list` \n List of 6 floats corresponding to impedances on translation \n (xyz) and rotation about (xyz) axes. Default is None. If None \n then will use default impedances.\n joint_impedances : :obj:`list` \n List of 7 floats corresponding to impedances on each joint. \n This is used when use_impedance is False. Default is None. \n If None then will use default impedances.\n k_gains : :obj:`list` \n List of 7 floats corresponding to the k_gains on each joint for \n our impedance controller. This is used when use_impedance is \n True. Default is None. If None then will use default k_gains.\n d_gains : :obj:`list` \n List of 7 floats corresponding to the d_gains on each joint for \n our impedance controller. This is used when use_impedance is \n True. Default is None. If None then will use default d_gains.\n block : :obj:`bool` \n Function blocks by default. If False, the function becomes\n asynchronous and can be preempted.\n ignore_errors : :obj:`bool` \n Function ignores errors by default. If False, errors and some \n exceptions can be thrown.\n ignore_virtual_walls : :obj:`bool` \n Function checks for collisions with virtual walls by default. \n If False, the robot no longer checks, which may be dangerous.\n skill_desc : :obj:`str` \n Skill description to use for logging on the Control PC.\n\n Raises:\n ValueError: If is_joints_reachable(joints) returns False\n \"\"\"\n\n joints = np.array(joints).tolist() \n\n if dynamic:\n skill = Skill(SkillType.ImpedanceControlSkill, \n TrajectoryGeneratorType.PassThroughJointTrajectoryGenerator,\n feedback_controller_type=FeedbackControllerType.JointImpedanceFeedbackController,\n termination_handler_type=TerminationHandlerType.TimeTerminationHandler, \n skill_desc=skill_desc)\n use_impedance=True\n block = False\n else:\n if use_impedance:\n skill = Skill(SkillType.ImpedanceControlSkill, \n TrajectoryGeneratorType.MinJerkJointTrajectoryGenerator,\n feedback_controller_type=FeedbackControllerType.JointImpedanceFeedbackController,\n termination_handler_type=TerminationHandlerType.FinalJointTerminationHandler, \n skill_desc=skill_desc)\n else:\n skill = Skill(SkillType.JointPositionSkill, \n TrajectoryGeneratorType.MinJerkJointTrajectoryGenerator,\n feedback_controller_type=FeedbackControllerType.SetInternalImpedanceFeedbackController,\n termination_handler_type=TerminationHandlerType.FinalJointTerminationHandler, \n skill_desc=skill_desc)\n\n\n if not self.is_joints_reachable(joints):\n raise ValueError('Joints not reachable!')\n if not ignore_virtual_walls and self.is_joints_in_collision_with_boxes(joints):\n raise ValueError('Target joints in collision with virtual walls!')\n\n skill.add_initial_sensor_values(FC.EMPTY_SENSOR_VALUES)\n\n skill.set_joint_impedances(use_impedance, cartesian_impedances, joint_impedances, k_gains, d_gains)\n\n if not skill.check_for_contact_params(buffer_time, force_thresholds, torque_thresholds):\n if dynamic:\n skill.add_time_termination_params(buffer_time)\n else:\n skill.add_joint_threshold_params(buffer_time, FC.DEFAULT_JOINT_THRESHOLDS)\n\n skill.add_goal_joints(duration, joints)\n goal = skill.create_goal()\n\n self._send_goal(goal,\n cb=lambda x: skill.feedback_callback(x),\n block=block,\n ignore_errors=ignore_errors)\n\n if dynamic:\n sleep(FC.DYNAMIC_SKILL_WAIT_TIME)\n\n def execute_cartesian_velocities(self,\n cartesian_velocities,\n cartesian_accelerations,\n duration=5,\n dynamic=False,\n buffer_time=FC.DEFAULT_TERM_BUFFER_TIME,\n force_thresholds=None,\n torque_thresholds=None,\n cartesian_impedances=None,\n joint_impedances=None,\n block=True,\n ignore_errors=True,\n ignore_virtual_walls=False,\n skill_desc='ExecutecartesianVelocities'):\n \"\"\"\n Commands Arm to execute cartesian velocities\n\n Parameters\n ----------\n cartesian_velocities : :obj:`list` \n A list of 6 numbers that correspond to cartesian velocities in m/s and rad/s.\n cartesian_accelerations : :obj:`list` \n A list of 6 numbers that correspond to cartesian accelerations in m/s^2 and rad/s^2.\n duration : :obj:`float` \n How much time this robot motion should take.\n dynamic : :obj:`bool` \n Flag that states whether the skill is dynamic. If True, it \n will use our cartesian impedance controller and sensor values.\n buffer_time : :obj:`float` \n How much extra time the termination handler will wait\n before stopping the skill after duration has passed.\n force_thresholds : :obj:`list` \n List of 6 floats corresponding to force limits on translation \n (xyz) and rotation about (xyz) axes. Default is None. \n If None then will not stop on contact.\n torque_thresholds : :obj:`list` \n List of 7 floats corresponding to torque limits on each joint. \n Default is None. If None then will not stop on contact.\n cartesian_impedances : :obj:`list` \n List of 6 floats corresponding to impedances on translation \n (xyz) and rotation about (xyz) axes. Default is None. If None \n then will use default impedances.\n joint_impedances : :obj:`list` \n List of 7 floats corresponding to impedances on each joint. \n This is used when use_impedance is False. Default is None. \n If None then will use default impedances.\n block : :obj:`bool` \n Function blocks by default. If False, the function becomes\n asynchronous and can be preempted.\n ignore_errors : :obj:`bool` \n Function ignores errors by default. If False, errors and some \n exceptions can be thrown.\n ignore_virtual_walls : :obj:`bool` \n Function checks for collisions with virtual walls by default. \n If False, the robot no longer checks, which may be dangerous.\n skill_desc : :obj:`str` \n Skill description to use for logging on the Control PC.\n\n Raises:\n ValueError: If is_cartesians_reachable(cartesians) returns False\n \"\"\"\n\n cartesian_velocities = np.array(cartesian_velocities).tolist() \n cartesian_accelerations = np.array(cartesian_accelerations).tolist() \n\n if dynamic:\n skill = Skill(SkillType.CartesianVelocitySkill, \n TrajectoryGeneratorType.PassThroughCartesianVelocityTrajectoryGenerator,\n feedback_controller_type=FeedbackControllerType.SetInternalImpedanceFeedbackController,\n termination_handler_type=TerminationHandlerType.TimeTerminationHandler, \n skill_desc=skill_desc)\n block = False\n else:\n print(\"Unimplemented Cartesian Velocity Skill\")\n\n skill.add_initial_sensor_values(FC.EMPTY_SENSOR_VALUES)\n\n skill.set_cartesian_impedances(False, cartesian_impedances, cartesian_impedances)\n\n if not skill.check_for_contact_params(buffer_time, force_thresholds, torque_thresholds):\n if dynamic:\n skill.add_time_termination_params(buffer_time)\n \n skill.add_goal_cartesian_velocities(duration, cartesian_velocities, cartesian_accelerations)\n goal = skill.create_goal()\n\n self._send_goal(goal,\n cb=lambda x: skill.feedback_callback(x),\n block=block,\n ignore_errors=ignore_errors)\n\n if dynamic:\n sleep(FC.DYNAMIC_SKILL_WAIT_TIME)\n\n def execute_joint_velocities(self,\n joint_velocities,\n joint_accelerations,\n duration=5,\n dynamic=False,\n buffer_time=FC.DEFAULT_TERM_BUFFER_TIME,\n force_thresholds=None,\n torque_thresholds=None,\n cartesian_impedances=None,\n joint_impedances=None,\n block=True,\n ignore_errors=True,\n ignore_virtual_walls=False,\n skill_desc='ExecuteJointVelocities'):\n \"\"\"\n Commands Arm to execute joint velocities\n\n Parameters\n ----------\n joint_velocities : :obj:`list` \n A list of 7 numbers that correspond to joint velocities in radians/second.\n joint_accelerations : :obj:`list` \n A list of 7 numbers that correspond to joint accelerations in radians/second^2.\n duration : :obj:`float` \n How much time this robot motion should take.\n dynamic : :obj:`bool` \n Flag that states whether the skill is dynamic. If True, it \n will use our joint impedance controller and sensor values.\n buffer_time : :obj:`float` \n How much extra time the termination handler will wait\n before stopping the skill after duration has passed.\n force_thresholds : :obj:`list` \n List of 6 floats corresponding to force limits on translation \n (xyz) and rotation about (xyz) axes. Default is None. \n If None then will not stop on contact.\n torque_thresholds : :obj:`list` \n List of 7 floats corresponding to torque limits on each joint. \n Default is None. If None then will not stop on contact.\n cartesian_impedances : :obj:`list` \n List of 6 floats corresponding to impedances on translation \n (xyz) and rotation about (xyz) axes. Default is None. If None \n then will use default impedances.\n joint_impedances : :obj:`list` \n List of 7 floats corresponding to impedances on each joint. \n This is used when use_impedance is False. Default is None. \n If None then will use default impedances.\n block : :obj:`bool` \n Function blocks by default. If False, the function becomes\n asynchronous and can be preempted.\n ignore_errors : :obj:`bool` \n Function ignores errors by default. If False, errors and some \n exceptions can be thrown.\n ignore_virtual_walls : :obj:`bool` \n Function checks for collisions with virtual walls by default. \n If False, the robot no longer checks, which may be dangerous.\n skill_desc : :obj:`str` \n Skill description to use for logging on the Control PC.\n\n Raises:\n ValueError: If is_joints_reachable(joints) returns False\n \"\"\"\n\n joint_velocities = np.array(joint_velocities).tolist() \n joint_accelerations = np.array(joint_accelerations).tolist() \n\n if dynamic:\n skill = Skill(SkillType.JointVelocitySkill, \n TrajectoryGeneratorType.PassThroughJointVelocityTrajectoryGenerator,\n feedback_controller_type=FeedbackControllerType.SetInternalImpedanceFeedbackController,\n termination_handler_type=TerminationHandlerType.TimeTerminationHandler, \n skill_desc=skill_desc)\n block = False\n else:\n print(\"Unimplemented Joint Velocity Skill\")\n\n skill.add_initial_sensor_values(FC.EMPTY_SENSOR_VALUES)\n\n skill.set_joint_impedances(False, cartesian_impedances, joint_impedances, None, None)\n\n if not skill.check_for_contact_params(buffer_time, force_thresholds, torque_thresholds):\n if dynamic:\n skill.add_time_termination_params(buffer_time)\n else:\n print(\"Unimplemented Joint Velocity Skill\")\n\n skill.add_goal_joint_velocities(duration, joint_velocities, joint_accelerations)\n goal = skill.create_goal()\n\n self._send_goal(goal,\n cb=lambda x: skill.feedback_callback(x),\n block=block,\n ignore_errors=ignore_errors)\n\n if dynamic:\n sleep(FC.DYNAMIC_SKILL_WAIT_TIME)\n\n def execute_joint_torques(self,\n joint_torques,\n selection=[0,0,0,0,0,0,0],\n remove_gravity=[0,0,0,0,0,0,0],\n duration=5,\n dynamic=False,\n buffer_time=FC.DEFAULT_TERM_BUFFER_TIME,\n force_thresholds=None,\n torque_thresholds=None,\n k_gains=None,\n d_gains=None,\n block=True,\n ignore_errors=True,\n ignore_virtual_walls=False,\n skill_desc='ExecuteJointTorques'):\n \"\"\"\n Commands Arm to execute joint torques\n\n Parameters\n ----------\n joint_torques : :obj:`list` \n A list of 7 numbers that correspond to joint torques in radians/second.\n selection : :obj:`list` \n A list of 7 numbers that indicate whether to use the joint torques passed in or not. \n If 1 is passed in for a specific joint, the robot will use the joint torque for that joint.\n remove_gravity : :obj:`list` \n A list of 7 numbers that indicate whether to remove gravity from that joint or not. \n If 1 is passed in for a specific joint, the robot will subtract gravity for that joint.\n duration : :obj:`float` \n How much time this robot motion should take.\n dynamic : :obj:`bool` \n Flag that states whether the skill is dynamic. If True, it \n will use our joint impedance controller and sensor values.\n buffer_time : :obj:`float` \n How much extra time the termination handler will wait\n before stopping the skill after duration has passed.\n force_thresholds : :obj:`list` \n List of 6 floats corresponding to force limits on translation \n (xyz) and rotation about (xyz) axes. Default is None. \n If None then will not stop on contact.\n torque_thresholds : :obj:`list` \n List of 7 floats corresponding to torque limits on each joint. \n Default is None. If None then will not stop on contact.\n k_gains : :obj:`list` \n List of 7 floats corresponding to the k_gains on each joint for \n our impedance controller. Default is None. If None then will use \n default k_gains.\n d_gains : :obj:`list` \n List of 7 floats corresponding to the d_gains on each joint for \n our impedance controller. Default is None. If None then will use \n default d_gains.\n block : :obj:`bool` \n Function blocks by default. If False, the function becomes\n asynchronous and can be preempted.\n ignore_errors : :obj:`bool` \n Function ignores errors by default. If False, errors and some \n exceptions can be thrown.\n ignore_virtual_walls : :obj:`bool` \n Function checks for collisions with virtual walls by default. \n If False, the robot no longer checks, which may be dangerous.\n skill_desc : :obj:`str` \n Skill description to use for logging on the Control PC.\n\n Raises:\n ValueError: If is_joints_reachable(joints) returns False\n \"\"\"\n\n joint_torques = np.array(joint_torques).tolist()\n selection = np.array(selection).tolist() \n\n if dynamic:\n block = False\n else:\n block = True\n\n if k_gains is None:\n k_gains = FC.DEFAULT_K_GAINS\n else:\n k_gains = np.array(k_gains).tolist()\n if d_gains is None:\n d_gains = FC.DEFAULT_D_GAINS\n else:\n d_gains = np.array(d_gains).tolist()\n\n skill = Skill(SkillType.ImpedanceControlSkill, \n TrajectoryGeneratorType.StayInInitialJointsTrajectoryGenerator,\n feedback_controller_type=FeedbackControllerType.PassThroughJointTorqueFeedbackController,\n termination_handler_type=TerminationHandlerType.TimeTerminationHandler, \n skill_desc=skill_desc)\n\n skill.add_initial_sensor_values(FC.EMPTY_SENSOR_VALUES)\n\n skill.add_run_time(duration)\n\n if not skill.check_for_contact_params(buffer_time, force_thresholds, torque_thresholds):\n skill.add_time_termination_params(buffer_time)\n\n skill.add_joint_torques(joint_torques, selection, remove_gravity, k_gains, d_gains)\n\n goal = skill.create_goal()\n\n self._send_goal(goal,\n cb=lambda x: skill.feedback_callback(x),\n block=block,\n ignore_errors=ignore_errors)\n\n if dynamic:\n sleep(FC.DYNAMIC_SKILL_WAIT_TIME)\n\n def execute_joint_dmp(self, \n joint_dmp_info, \n duration, \n initial_sensor_values=None,\n use_impedance=False, \n buffer_time=FC.DEFAULT_TERM_BUFFER_TIME,\n force_thresholds=None,\n torque_thresholds=None,\n cartesian_impedances=None,\n joint_impedances=None, \n k_gains=None, \n d_gains=None, \n block=True, \n ignore_errors=True,\n skill_desc='JointDmp'):\n \"\"\"\n Commands Arm to execute a given joint dmp\n\n Parameters\n ----------\n joint_dmp_info : :obj:`dict` \n Contains all the parameters of a joint DMP\n (tau, alpha, beta, num_basis, num_sensors, mu, h, and weights)\n duration : :obj:`float` \n How much time this robot motion should take.\n initial_sensor_values : :obj:`list` \n List of initial sensor values. If None it will default to ones.\n use_impedance : :obj:`bool` \n Function uses the Franka joint impedance controller by default. \n If True, uses our joint impedance controller.\n buffer_time : :obj:`float` \n How much extra time the termination handler will wait\n before stopping the skill after duration has passed.\n force_thresholds : :obj:`list` \n List of 6 floats corresponding to force limits on translation \n (xyz) and rotation about (xyz) axes. Default is None. \n If None then will not stop on contact.\n torque_thresholds : :obj:`list` \n List of 7 floats corresponding to torque limits on each joint. \n Default is None. If None then will not stop on contact.\n cartesian_impedances : :obj:`list` \n List of 6 floats corresponding to impedances on translation \n (xyz) and rotation about (xyz) axes. Default is None. \n If None then will use default impedances.\n joint_impedances : :obj:`list` \n List of 7 floats corresponding to impedances on each joint. \n This is used when use_impedance is False. Default is None. \n If None then will use default impedances.\n k_gains : :obj:`list` \n List of 7 floats corresponding to the k_gains on each joint\n for our impedance controller. This is used when use_impedance is \n True. Default is None. If None then will use default k_gains.\n d_gains : :obj:`list` \n List of 7 floats corresponding to the d_gains on each joint\n for our impedance controller. This is used when use_impedance is \n True. Default is None. If None then will use default d_gains.\n block : :obj:`bool`\n Function blocks by default. If False, the function \n becomes asynchronous and can be preempted.\n ignore_errors : :obj:`bool`\n Function ignores errors by default. \n If False, errors and some exceptions can be thrown.\n skill_desc : :obj:`str` \n Skill description to use for logging on control-pc.\n \"\"\"\n\n if use_impedance:\n skill = Skill(SkillType.ImpedanceControlSkill, \n TrajectoryGeneratorType.JointDmpTrajectoryGenerator,\n feedback_controller_type=FeedbackControllerType.JointImpedanceFeedbackController,\n termination_handler_type=TerminationHandlerType.FinalJointTerminationHandler, \n skill_desc=skill_desc)\n else:\n skill = Skill(SkillType.JointPositionSkill, \n TrajectoryGeneratorType.JointDmpTrajectoryGenerator,\n feedback_controller_type=FeedbackControllerType.SetInternalImpedanceFeedbackController,\n termination_handler_type=TerminationHandlerType.FinalJointTerminationHandler, \n skill_desc=skill_desc)\n\n if initial_sensor_values is None:\n initial_sensor_values = np.ones(joint_dmp_info['num_sensors']).tolist()\n\n skill.add_initial_sensor_values(initial_sensor_values) # sensor values\n \n skill.add_joint_dmp_params(duration, joint_dmp_info, initial_sensor_values)\n \n skill.set_joint_impedances(use_impedance, cartesian_impedances, joint_impedances, k_gains, d_gains)\n\n if not skill.check_for_contact_params(buffer_time, force_thresholds, torque_thresholds):\n skill.add_time_termination_params(buffer_time)\n\n goal = skill.create_goal()\n\n self._send_goal(goal,\n cb=lambda x: skill.feedback_callback(x),\n block=block,\n ignore_errors=ignore_errors)\n\n def execute_pose_dmp(self, \n pose_dmp_info, \n duration, \n use_goal_formulation=False,\n initial_sensor_values=None,\n orientation_only = False,\n position_only = False,\n ee_frame = False,\n use_impedance=True, \n buffer_time=FC.DEFAULT_TERM_BUFFER_TIME,\n force_thresholds=None,\n torque_thresholds=None,\n cartesian_impedances=None,\n joint_impedances=None, \n block=True, \n ignore_errors=True,\n skill_desc='PoseDmp'):\n \"\"\"\n Commands Arm to execute a given pose dmp\n\n Parameters\n ----------\n pose_dmp_info : :obj:`dict`\n Contains all the parameters of a pose DMP\n (tau, alpha, beta, num_basis, num_sensors, mu, h, and weights)\n duration : :obj:`float`\n How much time this robot motion should take.\n use_goal_formulation : :obj:`bool`\n Flag that represents whether to use the explicit goal pose \n dmp formulation.\n initial_sensor_values : :obj:`list` \n List of initial sensor values. If None it will default to ones.\n orientation_only : :obj:`bool`\n Flag that represents if the dmp weights are to generate a dmp \n only for orientation.\n position_only : :obj:`bool`\n Flag that represents if the dmp weights are to generate a dmp \n only for position.\n ee_frame : :obj:`bool`\n Flag that indicates whether the dmp is in terms of the \n end-effector frame.\n use_impedance : :obj:`bool`\n Function uses our impedance controller by default. If False, \n uses the Franka cartesian controller.\n buffer_time : :obj:`float`\n How much extra time the termination handler will wait\n before stopping the skill after duration has passed.\n force_thresholds : :obj:`list` \n List of 6 floats corresponding to force limits on translation \n (xyz) and rotation about (xyz) axes. Default is None. \n If None then will not stop on contact.\n torque_thresholds : :obj:`list` \n List of 7 floats corresponding to torque limits on each joint. \n Default is None. If None then will not stop on contact.\n cartesian_impedances : :obj:`list` \n List of 6 floats corresponding to impedances on translation \n (xyz) and rotation about (xyz) axes. Default is None. \n If None then will use default impedances.\n joint_impedances : :obj:`list` \n List of 7 floats corresponding to impedances on each joint. \n This is used when use_impedance is False. Default is None. \n If None then will use default impedances.\n block : :obj:`bool`\n Function blocks by default. If False, the function becomes\n asynchronous and can be preempted.\n ignore_errors : :obj:`bool`\n Function ignores errors by default. If False, errors and \n some exceptions can be thrown.\n skill_desc : :obj:`str` \n Skill description to use for logging on control-pc.\n \"\"\"\n\n if use_impedance:\n if use_goal_formulation:\n skill = Skill(SkillType.ImpedanceControlSkill, \n TrajectoryGeneratorType.GoalPoseDmpTrajectoryGenerator,\n feedback_controller_type=FeedbackControllerType.CartesianImpedanceFeedbackController,\n termination_handler_type=TerminationHandlerType.TimeTerminationHandler, \n skill_desc=skill_desc)\n else:\n skill = Skill(SkillType.ImpedanceControlSkill, \n TrajectoryGeneratorType.PoseDmpTrajectoryGenerator,\n feedback_controller_type=FeedbackControllerType.CartesianImpedanceFeedbackController,\n termination_handler_type=TerminationHandlerType.TimeTerminationHandler, \n skill_desc=skill_desc)\n else:\n if use_goal_formulation:\n skill = Skill(SkillType.CartesianPoseSkill, \n TrajectoryGeneratorType.GoalPoseDmpTrajectoryGenerator,\n feedback_controller_type=FeedbackControllerType.SetInternalImpedanceFeedbackController,\n termination_handler_type=TerminationHandlerType.TimeTerminationHandler, \n skill_desc=skill_desc)\n else:\n skill = Skill(SkillType.CartesianPoseSkill, \n TrajectoryGeneratorType.PoseDmpTrajectoryGenerator,\n feedback_controller_type=FeedbackControllerType.SetInternalImpedanceFeedbackController,\n termination_handler_type=TerminationHandlerType.TimeTerminationHandler, \n skill_desc=skill_desc)\n\n if initial_sensor_values is None:\n if orientation_only or position_only:\n initial_sensor_values = np.ones(3*pose_dmp_info['num_sensors']).tolist()\n else:\n initial_sensor_values = np.ones(6*pose_dmp_info['num_sensors']).tolist()\n\n skill.add_initial_sensor_values(initial_sensor_values) # sensor values\n\n skill.add_pose_dmp_params(orientation_only, position_only, ee_frame, duration, pose_dmp_info, initial_sensor_values)\n\n skill.set_cartesian_impedances(use_impedance, cartesian_impedances, joint_impedances)\n\n if not skill.check_for_contact_params(buffer_time, force_thresholds, torque_thresholds):\n skill.add_time_termination_params(buffer_time)\n\n goal = skill.create_goal()\n\n self._send_goal(goal,\n cb=lambda x: skill.feedback_callback(x),\n block=block,\n ignore_errors=ignore_errors)\n\n def execute_quaternion_pose_dmp(self, \n position_dmp_info,\n quat_dmp_info,\n duration,\n goal_quat,\n goal_quat_time, \n use_goal_formulation=False,\n initial_sensor_values=None,\n ee_frame=False,\n use_impedance=True, \n buffer_time=FC.DEFAULT_TERM_BUFFER_TIME,\n force_thresholds=None,\n torque_thresholds=None,\n cartesian_impedances=None,\n joint_impedances=None, \n block=True, \n ignore_errors=True,\n skill_desc='QuaternionPoseDmp'):\n '''Commands Arm to execute a given quaternion pose dmp\n\n Args:\n position_dmp_info : :obj:`dict`\n Contains all the parameters of a position DMP\n (tau, alpha, beta, num_basis, num_sensors, mu, h, and weights)\n quat_dmp_info : :obj:`dict`\n Contains all the parameters of a quaternion DMP\n (tau, alpha, beta, num_basis, num_sensors, mu, h, and weights)\n duration : :obj:`float`\n How much time this robot motion should take.\n goal_quat : :obj:`list` \n List of 4 floats that represents the quaternion goal to reach. \n This assumes an explicit goal formulation for the quaternion DMPs.\n goal_quat_time : :obj:`float`\n Time to reach goal for quaternion DMPs.\n use_goal_formulation : :obj:`bool`\n Flag that represents whether to use the explicit goal pose dmp formulation.\n initial_sensor_values : :obj:`list` \n List of initial sensor values. If None it will default to ones.\n ee_frame : :obj:`bool`\n Flag that indicates whether the dmp is in terms of the \n end-effector frame.\n use_impedance : :obj:`bool`\n Function uses our impedance controller by default. If False, \n uses the Franka cartesian controller.\n buffer_time : :obj:`float`\n How much extra time the termination handler will wait\n before stopping the skill after duration has passed.\n force_thresholds : :obj:`list` \n List of 6 floats corresponding to force limits on translation \n (xyz) and rotation about (xyz) axes. Default is None. \n If None then will not stop on contact.\n torque_thresholds : :obj:`list` \n List of 7 floats corresponding to torque limits on each joint. \n Default is None. If None then will not stop on contact.\n cartesian_impedances : :obj:`list` \n List of 6 floats corresponding to impedances on translation \n (xyz) and rotation about (xyz) axes. Default is None. \n If None then will use default impedances.\n joint_impedances : :obj:`list` \n List of 7 floats corresponding to impedances on each joint. \n This is used when use_impedance is False. Default is None. \n If None then will use default impedances.\n block : :obj:`bool`\n Function blocks by default. If False, the function becomes\n asynchronous and can be preempted.\n ignore_errors : :obj:`bool`\n Function ignores errors by default. If False, errors and some\n exceptions can be thrown.\n skill_desc : :obj:`str` \n Skill description to use for logging on control-pc.\n '''\n\n if use_impedance:\n skill = Skill(SkillType.ImpedanceControlSkill, \n TrajectoryGeneratorType.QuaternionPoseDmpTrajectoryGenerator,\n feedback_controller_type=FeedbackControllerType.CartesianImpedanceFeedbackController,\n termination_handler_type=TerminationHandlerType.TimeTerminationHandler, \n skill_desc=skill_desc)\n else:\n skill = Skill(SkillType.CartesianPoseSkill, \n TrajectoryGeneratorType.QuaternionPoseDmpTrajectoryGenerator,\n feedback_controller_type=FeedbackControllerType.SetInternalImpedanceFeedbackController,\n termination_handler_type=TerminationHandlerType.TimeTerminationHandler, \n skill_desc=skill_desc)\n\n if initial_sensor_values is None:\n initial_sensor_values = np.ones(3*position_dmp_info['num_sensors']).tolist()\n\n skill.add_initial_sensor_values(initial_sensor_values) # sensor values\n\n skill.add_quaternion_pose_dmp_params(ee_frame, duration, position_dmp_info, quat_dmp_info, initial_sensor_values, goal_quat, goal_quat_time)\n skill.set_cartesian_impedances(use_impedance, cartesian_impedances, joint_impedances)\n\n if not skill.check_for_contact_params(buffer_time, force_thresholds, torque_thresholds):\n skill.add_time_termination_params(buffer_time)\n\n goal = skill.create_goal()\n\n self._send_goal(goal,\n cb=lambda x: skill.feedback_callback(x),\n block=block,\n ignore_errors=ignore_errors)\n\n def apply_effector_forces_torques(self,\n run_duration,\n acc_duration,\n max_translation,\n max_rotation,\n forces=None,\n torques=None,\n buffer_time=FC.DEFAULT_TERM_BUFFER_TIME,\n force_thresholds=None,\n torque_thresholds=None,\n block=True,\n ignore_errors=True,\n skill_desc='ForceTorque'):\n \"\"\"\n Applies the given end-effector forces and torques in N and Nm\n\n Parameters\n ----------\n run_duration : :obj:`float`\n How much time this robot motion should take.\n acc_duration : :obj:`float`\n How long to acc/de-acc to achieve desired force.\n max_translation : :obj:`float`\n A float in the unit of meters. Max translation before the \n robot deaccelerates.\n max_rotation : :obj:`float`\n A float in the unit of rad. Max rotation before the robot\n deaccelerates.\n forces : :obj:`list` \n Optional (defaults to None). A list of 3 numbers that correspond \n to end-effector forces in (xyz) directions.\n torques : :obj:`list` \n Optional (defaults to None). A list of 3 numbers that correspond \n to end-effector torques in 3 rotational axes.\n buffer_time : :obj:`float`\n How much extra time the termination handler will wait\n before stopping the skill after duration has passed.\n force_thresholds : :obj:`list` \n List of 6 floats corresponding to force limits on translation \n (xyz) and rotation about (xyz) axes. Default is None. \n If None then will not stop on contact.\n torque_thresholds : :obj:`list` \n List of 7 floats corresponding to torque limits on each joint. \n Default is None. If None then will not stop on contact.\n block : :obj:`bool`\n Function blocks by default. If False, the function becomes\n asynchronous and can be preempted.\n ignore_errors : :obj:`bool`\n Function ignores errors by default. If False, errors and some \n exceptions can be thrown.\n skill_desc : :obj:`str` \n Skill description to use for logging on control-pc.\n\n Raises:\n ValueError if acc_duration > 0.5*run_duration, or if forces are\n too large\n \"\"\"\n if acc_duration > 0.5 * run_duration:\n raise ValueError(\n 'acc_duration must be smaller than half of run_duration!')\n\n forces = [0, 0, 0] if forces is None else np.array(forces).tolist()\n torques = [0, 0, 0] if torques is None else np.array(torques).tolist()\n\n if np.linalg.norm(forces) * run_duration > FC.MAX_LIN_MOMENTUM:\n raise ValueError('Linear momentum magnitude exceeds safety '\n 'threshold of {}'.format(FC.MAX_LIN_MOMENTUM))\n if np.linalg.norm(torques) * run_duration > FC.MAX_ANG_MOMENTUM:\n raise ValueError('Angular momentum magnitude exceeds safety '\n 'threshold of {}'.format(FC.MAX_ANG_MOMENTUM))\n\n skill = Skill(SkillType.ForceTorqueSkill, \n TrajectoryGeneratorType.ImpulseTrajectoryGenerator,\n feedback_controller_type=FeedbackControllerType.PassThroughFeedbackController,\n termination_handler_type=TerminationHandlerType.TimeTerminationHandler, \n skill_desc=skill_desc)\n\n skill.add_initial_sensor_values(FC.EMPTY_SENSOR_VALUES)\n \n skill.add_impulse_params(run_duration, acc_duration, max_translation, max_rotation, forces, torques)\n\n if not skill.check_for_contact_params(buffer_time, force_thresholds, torque_thresholds):\n skill.add_time_termination_params(buffer_time)\n\n goal = skill.create_goal()\n\n self._send_goal(goal,\n cb=lambda x: skill.feedback_callback(x),\n block=block,\n ignore_errors=ignore_errors)\n\n def apply_effector_forces_along_axis(self,\n run_duration,\n acc_duration,\n max_translation,\n forces,\n buffer_time=FC.DEFAULT_TERM_BUFFER_TIME,\n force_thresholds=None,\n torque_thresholds=None,\n block=True,\n ignore_errors=True,\n skill_desc='ForcesAlongAxis'):\n \"\"\"\n Applies the given end-effector forces and torques in N and Nm\n\n Parameters\n ----------\n run_duration : :obj:`float`\n How much time this robot motion should take.\n acc_duration : :obj:`float`\n How long to acc/de-acc to achieve desired force.\n max_translation : :obj:`float`\n A float in the unit of meters. Max translation before the \n robot deaccelerates.\n forces : :obj:`list` \n Optional (defaults to None). A list of 3 numbers that correspond \n to the magnitude of the end-effector forces and the axis.\n buffer_time : :obj:`float`\n How much extra time the termination handler will wait\n before stopping the skill after duration has passed.\n force_thresholds : :obj:`list` \n List of 6 floats corresponding to force limits on translation \n (xyz) and rotation about (xyz) axes. Default is None. \n If None then will not stop on contact.\n torque_thresholds : :obj:`list` \n List of 7 floats corresponding to torque limits on each joint. \n Default is None. If None then will not stop on contact.\n block : :obj:`bool`\n Function blocks by default. If False, the function becomes\n asynchronous and can be preempted.\n ignore_errors : :obj:`bool`\n Function ignores errors by default. If False, errors and some \n exceptions can be thrown.\n skill_desc : :obj:`str` \n Skill description to use for logging on control-pc.\n \n Raises:\n ValueError if acc_duration > 0.5*run_duration, or if forces are\n too large\n \"\"\"\n if acc_duration > 0.5 * run_duration:\n raise ValueError(\n 'acc_duration must be smaller than half of run_duration!')\n if np.linalg.norm(forces) * run_duration > FC.MAX_LIN_MOMENTUM_CONSTRAINED:\n raise ValueError('Linear momentum magnitude exceeds safety ' \\\n 'threshold of {}'.format(FC.MAX_LIN_MOMENTUM_CONSTRAINED))\n\n forces = np.array(forces)\n force_axis = forces / np.linalg.norm(forces)\n\n skill = Skill(SkillType.ForceTorqueSkill, \n TrajectoryGeneratorType.ImpulseTrajectoryGenerator,\n feedback_controller_type=FeedbackControllerType.ForceAxisImpedenceFeedbackController,\n termination_handler_type=TerminationHandlerType.TimeTerminationHandler, \n skill_desc=skill_desc)\n\n skill.add_initial_sensor_values(FC.EMPTY_SENSOR_VALUES)\n \n skill.add_impulse_params(run_duration, acc_duration, max_translation, 0, forces.tolist(), [0, 0, 0])\n\n skill.add_force_axis_params(FC.DEFAULT_FORCE_AXIS_TRANSLATIONAL_STIFFNESS,\n FC.DEFAULT_FORCE_AXIS_ROTATIONAL_STIFFNESS,\n force_axis.tolist())\n\n if not skill.check_for_contact_params(buffer_time, force_thresholds, torque_thresholds):\n skill.add_time_termination_params(buffer_time)\n\n goal = skill.create_goal()\n\n self._send_goal(goal,\n cb=lambda x: skill.feedback_callback(x),\n block=block,\n ignore_errors=ignore_errors)\n\n def goto_gripper(self, \n width, \n grasp=False, \n speed=0.04, \n force=0.0,\n epsilon_inner=0.08,\n epsilon_outer=0.08, \n block=True, \n ignore_errors=True, \n skill_desc='GoToGripper'):\n \"\"\"\n Commands gripper to goto a certain width, applying up to the given\n (default is max) force if needed\n\n Parameters\n ----------\n width : :obj:`float`\n Desired width of the gripper in the unit of meters.\n grasp : :obj:`bool`\n Flag that signals whether to grasp.\n speed : :obj:`float`\n Gripper operation speed in meters per sec.\n force : :obj:`float`\n Max gripper force to apply in N. Default to None,\n which gives acceptable force.\n epsilon_inner : :obj:`float`\n Max tolerated deviation when the actual grasped \n width is smaller than the commanded grasp width.\n epsilon_outer : :obj:`float`\n Max tolerated deviation when the actual grasped \n width is larger than the commanded grasp width.\n block : :obj:`bool`\n Function blocks by default. If False, the function \n becomes asynchronous and can be preempted.\n ignore_errors : :obj:`bool`\n Function ignores errors by default. If False, errors \n and some exceptions can be thrown.\n skill_desc : :obj:`str` \n Skill description to use for logging on control-pc.\n\n Raises:\n ValueError: If width is less than 0 or greater than TODO(jacky)\n the maximum gripper opening\n \"\"\"\n\n if width < FC.GRIPPER_WIDTH_MIN or width > FC.GRIPPER_WIDTH_MAX:\n raise ValueError(\n 'gripper width must be within the gripper limits of ' \\\n '{} and {}'.format(FC.GRIPPER_WIDTH_MIN,FC.GRIPPER_WIDTH_MAX))\n\n if self._old_gripper:\n skill = Skill(SkillType.GripperSkill, \n TrajectoryGeneratorType.GripperTrajectoryGenerator, \n skill_desc=skill_desc)\n\n skill.add_initial_sensor_values(FC.EMPTY_SENSOR_VALUES)\n\n skill.add_gripper_params(grasp, width, speed, force)\n\n goal = skill.create_goal()\n\n self._send_goal(goal,\n cb=lambda x: skill.feedback_callback(x),\n block=block,\n ignore_errors=ignore_errors)\n # this is so the gripper state can be updated, which happens with a\n # small lag\n sleep(FC.GRIPPER_CMD_SLEEP_TIME)\n else:\n if grasp:\n grasp_skill = GraspGoal()\n grasp_skill.width = width\n grasp_skill.speed = speed\n grasp_skill.force = force\n grasp_skill.epsilon.inner = epsilon_inner\n grasp_skill.epsilon.outer = epsilon_outer\n self._gripper_grasp_client.send_goal(grasp_skill)\n else:\n move_skill = MoveGoal()\n move_skill.width = width\n move_skill.speed = speed\n self._gripper_move_client.send_goal(move_skill)\n if block:\n self.wait_for_gripper()\n\n def selective_guidance_mode(self, \n duration=5,\n use_joints=False, \n use_impedance=False,\n use_ee_frame=False,\n buffer_time=FC.DEFAULT_TERM_BUFFER_TIME,\n force_thresholds=None,\n torque_thresholds=None,\n cartesian_impedances=None,\n joint_impedances=None,\n k_gains=None,\n d_gains=None,\n block=True,\n ignore_errors=True,\n ignore_virtual_walls=False,\n skill_desc='SelectiveGuidance'):\n \"\"\"\n Commands the Arm to stay in its current position with selective impedances\n that allow guidance in either certain joints or in cartesian pose.\n\n Parameters\n ----------\n duration : :obj:`float`\n How much time this guidance should take\n use_joints : :obj:`bool`\n Function uses cartesian impedance controller by default. \n If True, it uses joint impedance.\n use_impedance : :obj:`bool`\n Function uses the Franka joint impedance controller by default. \n If True, uses our joint impedance controller.\n use_ee_frame : :obj:`bool`\n Function uses the end-effector cartesian feedback controller\n only when use_impedance is True.\n buffer_time : :obj:`float`\n How much extra time the termination handler will wait\n before stopping the skill after duration has passed.\n force_thresholds : :obj:`list` \n List of 6 floats corresponding to force limits on translation \n (xyz) and rotation about (xyz) axes. Default is None. \n If None then will not stop on contact.\n torque_thresholds : :obj:`list` \n List of 7 floats corresponding to torque limits on each joint. \n Default is None. If None then will not stop on contact.\n cartesian_impedances : :obj:`list` \n List of 6 floats corresponding to impedances on translation \n (xyz) and rotation about (xyz) axes. Default is None. \n If None then will use default impedances.\n joint_impedances : :obj:`list` \n List of 7 floats corresponding to impedances on each joint. \n This is used when use_impedance is False. Default is None. \n If None then will use default impedances.\n k_gains : :obj:`list` \n List of 7 floats corresponding to the k_gains on each joint\n for our impedance controller. This is used when use_impedance is \n True. Default is None. If None then will use default k_gains.\n d_gains : :obj:`list` \n List of 7 floats corresponding to the d_gains on each joint\n for our impedance controller. This is used when use_impedance is \n True. Default is None. If None then will use default d_gains.\n block : :obj:`bool`\n Function blocks by default. If False, the function becomes\n asynchronous and can be preempted.\n ignore_errors : :obj:`bool`\n Function ignores errors by default. If False, errors and some \n exceptions can be thrown.\n ignore_virtual_walls : :obj:`bool`\n Function checks for collisions with virtual walls by default. \n If False, the robot no longer checks, which may be dangerous.\n skill_desc : :obj:`str` \n Skill description to use for logging on control-pc.\n \"\"\"\n if use_joints:\n if use_impedance:\n skill = Skill(SkillType.ImpedanceControlSkill, \n TrajectoryGeneratorType.StayInInitialJointsTrajectoryGenerator,\n feedback_controller_type=FeedbackControllerType.JointImpedanceFeedbackController,\n termination_handler_type=TerminationHandlerType.TimeTerminationHandler, \n skill_desc=skill_desc)\n else:\n skill = Skill(SkillType.JointPositionSkill, \n TrajectoryGeneratorType.StayInInitialJointsTrajectoryGenerator,\n feedback_controller_type=FeedbackControllerType.SetInternalImpedanceFeedbackController,\n termination_handler_type=TerminationHandlerType.TimeTerminationHandler, \n skill_desc=skill_desc)\n else:\n if use_impedance:\n if use_ee_frame:\n skill = Skill(SkillType.ImpedanceControlSkill, \n TrajectoryGeneratorType.StayInInitialPoseTrajectoryGenerator,\n feedback_controller_type=FeedbackControllerType.EECartesianImpedanceFeedbackController,\n termination_handler_type=TerminationHandlerType.TimeTerminationHandler, \n skill_desc=skill_desc)\n else:\n skill = Skill(SkillType.ImpedanceControlSkill, \n TrajectoryGeneratorType.StayInInitialPoseTrajectoryGenerator,\n feedback_controller_type=FeedbackControllerType.CartesianImpedanceFeedbackController,\n termination_handler_type=TerminationHandlerType.TimeTerminationHandler, \n skill_desc=skill_desc)\n else:\n skill = Skill(SkillType.CartesianPoseSkill, \n TrajectoryGeneratorType.StayInInitialPoseTrajectoryGenerator,\n feedback_controller_type=FeedbackControllerType.SetInternalImpedanceFeedbackController,\n termination_handler_type=TerminationHandlerType.TimeTerminationHandler, \n skill_desc=skill_desc)\n\n skill.add_initial_sensor_values(FC.EMPTY_SENSOR_VALUES)\n \n skill.add_run_time(duration)\n\n if use_joints:\n skill.set_joint_impedances(use_impedance, cartesian_impedances, joint_impedances, k_gains, d_gains)\n else:\n skill.set_cartesian_impedances(use_impedance, cartesian_impedances, joint_impedances)\n\n if not skill.check_for_contact_params(buffer_time, force_thresholds, torque_thresholds):\n skill.add_time_termination_params(buffer_time)\n\n goal = skill.create_goal()\n\n self._send_goal(goal,\n cb=lambda x: skill.feedback_callback(x),\n block=block,\n ignore_errors=ignore_errors)\n\n def open_gripper(self, block=True, skill_desc='OpenGripper'):\n \"\"\"\n Opens gripper to maximum width\n\n Parameters\n ----------\n block : :obj:`bool`\n Function blocks by default. If False, the function becomes\n asynchronous and can be preempted.\n skill_desc : :obj:`str` \n Skill description to use for logging on control-pc.\n \"\"\"\n self.goto_gripper(FC.GRIPPER_WIDTH_MAX, block=block, skill_desc=skill_desc)\n\n def close_gripper(self, grasp=True, block=True, skill_desc='CloseGripper'):\n \"\"\"\n Closes the gripper as much as possible\n\n Parameters\n ----------\n grasp : :obj:`bool`\n Flag that signals whether to grasp.\n block : :obj:`bool`\n Function blocks by default. If False, the function becomes\n asynchronous and can be preempted.\n skill_desc : :obj:`str` \n Skill description to use for logging on control-pc.\n \"\"\"\n self.goto_gripper(FC.GRIPPER_WIDTH_MIN, grasp=grasp,\n force=FC.GRIPPER_MAX_FORCE if grasp else None,\n block=block, skill_desc=skill_desc)\n\n def home_gripper(self, block=True, skill_desc='HomeGripper'):\n \"\"\"\n Homes the gripper.\n\n Parameters\n ----------\n block : :obj:`bool`\n Function blocks by default. If False, the function becomes\n asynchronous and can be preempted.\n skill_desc : :obj:`str` \n Skill description to use for logging on control-pc.\n \"\"\"\n self._last_gripper_command = 'Homing'\n homing_skill = HomingGoal()\n self._gripper_homing_client.send_goal(homing_skill)\n if block:\n self.wait_for_gripper()\n\n def stop_gripper(self, block=True, skill_desc='StopGripper'):\n \"\"\"\n Stops the gripper.\n\n Parameters\n ----------\n block : :obj:`bool`\n Function blocks by default. If False, the function becomes\n asynchronous and can be preempted.\n skill_desc : :obj:`str` \n Skill description to use for logging on control-pc.\n \"\"\"\n self._last_gripper_command = 'Stop'\n stop_skill = StopGoal()\n self._gripper_stop_client.send_goal(stop_skill)\n if block:\n self.wait_for_gripper()\n\n def run_guide_mode(self, duration=10, block=True, skill_desc='GuideMode'):\n \"\"\"\n Runs guide mode which allows you to move the robot freely.\n \n Parameters\n ----------\n duration : :obj:`float`\n How much time this guidance should take\n block : :obj:`bool`\n Function blocks by default. If False, the function becomes\n asynchronous and can be preempted.\n skill_desc : :obj:`str` \n Skill description to use for logging on control-pc.\n \"\"\"\n self.apply_effector_forces_torques(duration, 0, 0, 0, block=block, skill_desc=skill_desc)\n\n def run_dynamic_force_position(self,\n duration=3,\n buffer_time=FC.DEFAULT_TERM_BUFFER_TIME,\n force_thresholds=None,\n torque_thresholds=None,\n position_kps_cart=FC.DEFAULT_TRANSLATIONAL_STIFFNESSES + FC.DEFAULT_ROTATIONAL_STIFFNESSES,\n force_kps_cart=FC.DEFAULT_HFPC_FORCE_GAIN,\n position_kps_joint=FC.DEFAULT_K_GAINS,\n force_kps_joint=FC.DEFAULT_HFPC_FORCE_GAIN,\n S=FC.DEFAULT_HFPC_S,\n interpolate=False,\n use_cartesian_gains=True,\n ignore_errors=True,\n ignore_virtual_walls=False,\n skill_desc=''):\n \"\"\"\n Commands Arm to run dynamic hybrid force position control\n\n Parameters\n ----------\n duration : :obj:`float`\n How much time this robot motion should take.\n use_impedance : :obj:`bool`\n Function uses our impedance controller by default. \n If False, uses the Franka cartesian controller.\n buffer_time : :obj:`float`\n How much extra time the termination handler will wait\n before stopping the skill after duration has passed.\n force_thresholds : :obj:`list` \n List of 6 floats corresponding to force limits on translation \n (xyz) and rotation about (xyz) axes. Default is None. \n If None then will not stop on contact.\n torque_thresholds : :obj:`list` \n List of 7 floats corresponding to torque limits on each joint. \n Default is None. If None then will not stop on contact.\n position_kp_cart : :obj:`list` \n List of 6 floats corresponding to proportional gain used for \n position errors in cartesian space.\n force_kp_cart : :obj:`list` \n List of 6 floats corresponding to proportional gain used for \n force errors in cartesian space.\n position_kp_joint : :obj:`list` \n List of 7 floats corresponding to proportional gain used for \n position errors in joint space.\n force_kp_joint : :obj:`list` \n List of 6 floats corresponding to proportional gain used for \n force errors in joint space.\n S : :obj:`list` \n List of 6 numbers between 0 and 1 for the HFPC selection matrix.\n interpolate : :obj:`bool`\n Whether or not to perform linear interpolation in between way points.\n use_cartesian_gains : :obj:`bool`\n Whether to use cartesian gains or joint gains.\n ignore_errors : :obj:`bool`\n Function ignores errors by default. If False, errors and some \n exceptions can be thrown.\n ignore_virtual_walls : :obj:`bool`\n Function checks for collisions with virtual walls by default. \n If False, the robot no longer checks, which may be dangerous.\n skill_desc : :obj:`str` \n Skill description to use for logging on control-pc.\n \"\"\"\n if interpolate:\n traj_gen = TrajectoryGeneratorType.LinearForcePositionTrajectoryGenerator\n else:\n traj_gen = TrajectoryGeneratorType.PassThroughForcePositionTrajectoryGenerator\n skill = Skill(SkillType.ForceTorqueSkill, \n traj_gen,\n feedback_controller_type=FeedbackControllerType.ForcePositionFeedbackController,\n termination_handler_type=TerminationHandlerType.TimeTerminationHandler, \n skill_desc=skill_desc)\n\n skill.add_initial_sensor_values(FC.EMPTY_SENSOR_VALUES)\n skill.add_force_position_params(position_kps_cart, force_kps_cart, position_kps_joint, force_kps_joint, S, use_cartesian_gains)\n skill.add_run_time(duration)\n\n if not skill.check_for_contact_params(buffer_time, force_thresholds, torque_thresholds):\n skill.add_time_termination_params(buffer_time)\n \n goal = skill.create_goal()\n\n self._send_goal(goal,\n cb=lambda x: skill.feedback_callback(x),\n block=False,\n ignore_errors=ignore_errors)\n\n sleep(FC.DYNAMIC_SKILL_WAIT_TIME)\n\n \"\"\"\n Reads\n \"\"\"\n\n def get_robot_state(self):\n \"\"\"\n Returns\n -------\n robot_state : :obj:`dict`\n Dict that contains all of the robot's current\n state information.\n \"\"\"\n return self._state_client.get_data()\n\n def get_pose(self, include_tool_offset=True):\n \"\"\"\n Returns\n -------\n pose : :obj:`autolab_core.RigidTransform`\n Current pose of the end-effector including the transform\n to the end of the tool.\n \"\"\"\n tool_base_pose = self._state_client.get_pose()\n\n if include_tool_offset:\n tool_pose = tool_base_pose * self._tool_delta_pose\n return tool_pose\n else:\n return tool_base_pose\n\n def get_joints(self):\n \"\"\"\n Returns the current joint positions.\n\n Returns\n -------\n joint_positions : :obj:`numpy.ndarray`\n 7 floats that represent each joint's position in radians.\n \"\"\"\n return self._state_client.get_joints()\n\n def get_joint_torques(self):\n \"\"\"\n Returns the current joint torques.\n\n Returns\n -------\n joint_torques : :obj:`numpy.ndarray`\n 7 floats that represent each joint's torque in Nm.\n \"\"\"\n return self._state_client.get_joint_torques()\n\n def get_joint_velocities(self):\n \"\"\"\n Returns the current joint velocities.\n\n Returns\n -------\n joint_velocities : :obj:`numpy.ndarray`\n 7 floats that represent each joint's velocity in rads/s.\n \"\"\"\n return self._state_client.get_joint_velocities()\n\n def get_gripper_width(self):\n \"\"\"\n Returns the current gripper width.\n\n Returns\n -------\n gripper_width : :obj:`float`\n Robot gripper width in meters.\n \"\"\"\n if self._old_gripper:\n return self._state_client.get_gripper_width()\n else:\n gripper_data = rospy.wait_for_message(self._gripper_joint_states_name, JointState)\n return gripper_data.position[0] + gripper_data.position[1]\n\n def get_gripper_is_grasped(self):\n \"\"\"\n Returns a flag that represents if the gripper is grasping something.\n\n WARNING: THIS FUNCTION HAS BEEN DEPRECATED WHEN WE SWITCHED\n TO THE FRANKA_ROS GRIPPER CONTROLLER BECAUSE IT DOES NOT PUBLISH\n THE IS GRASPED FLAG. YOU WILL NEED TO DETERMINE WHEN THE ROBOT\n IS GRASPING AN ITEM YOURSELF USING THE GRIPPER WIDTH.\n\n Returns\n -------\n is_grasped : :obj:`bool`\n Returns True if gripper is grasping something. False otherwise.\n \"\"\"\n return self._state_client.get_gripper_is_grasped()\n\n def get_tool_base_pose(self):\n \"\"\"\n Returns the tool delta pose.\n\n Returns\n -------\n tool_delta_pose : :obj:`autolab_core.RigidTransform`\n RigidTransform that represents the transform between the \n end-effector and the end of the tool.\n \"\"\"\n return self._tool_delta_pose.copy()\n\n def get_ee_force_torque(self):\n \"\"\"\n Returns the current end-effector's sensed force_torque.\n\n Returns\n -------\n ee_force_torque : :obj:`numpy.ndarray`\n A numpy ndarray of 6 floats that represents the current\n end-effector's sensed force_torque.\n \"\"\"\n return self._state_client.get_ee_force_torque()\n\n def get_finger_poses(self):\n \"\"\"\n Returns the current poses of the left and right fingers.\n\n Returns\n -------\n left_finger_pose : :obj:`autolab_core.RigidTransform`\n RigidTransform that represents the transform of the left finger.\n The left finger is +y from the gripper pose\n right_finger_pose : :obj:`autolab_core.RigidTransform`\n RigidTransform that represents the transform of the right finger.\n The right finger is -y from the gripper pose\n \"\"\"\n ee_pose = self.get_pose()\n delta_gripper_width = self.get_gripper_width() / 2\n\n left_finger_pose = ee_pose * RigidTransform(\n translation=np.array([0, delta_gripper_width, 0]),\n from_frame='finger_left', to_frame=ee_pose.from_frame\n )\n right_finger_pose = ee_pose * RigidTransform(\n translation=np.array([0, -delta_gripper_width, 0]),\n from_frame='finger_right', to_frame=ee_pose.from_frame\n )\n return left_finger_pose, right_finger_pose\n\n \"\"\"\n Sets\n \"\"\"\n\n def set_tool_delta_pose(self, tool_delta_pose):\n \"\"\"\n Sets the tool pose relative to the end-effector pose\n\n Parameters\n ----------\n tool_delta_pose : :obj:`autolab_core.RigidTransform`\n RigidTransform that represents the transform between the \n end-effector and the end of the tool.\n \"\"\"\n if tool_delta_pose.from_frame != 'franka_tool' \\\n or tool_delta_pose.to_frame != 'franka_tool_base':\n raise ValueError('tool_delta_pose has invalid frame names! ' \\\n 'Make sure it has from_frame=franka_tool, and ' \\\n 'to_frame=franka_tool_base')\n\n self._tool_delta_pose = tool_delta_pose.copy()\n\n\n \"\"\"\n Forward Kinematics, Jacobian, other offline methods\n \"\"\"\n\n _dh_alpha_rot = np.array([\n [1, 0, 0, 0],\n [0, -1, -1, 0],\n [0, -1, -1, 0],\n [0, 0, 0, 1]\n ], dtype=np.float32)\n _dh_a_trans = np.array([\n [1, 0, 0, -1],\n [0, 1, 0, 0],\n [0, 0, 1, 0],\n [0, 0, 0, 1]\n ], dtype=np.float32)\n _dh_d_trans = np.array([\n [1, 0, 0, 0],\n [0, 1, 0, 0],\n [0, 0, 1, -1],\n [0, 0, 0, 1]\n ], dtype=np.float32)\n _dh_theta_rot = np.array([\n [-1, -1, 0, 0],\n [-1, -1, 0, 0],\n [0, 0, 1, 0],\n [0, 0, 0, 1]\n ], dtype=np.float32)\n\n def get_links_transforms(self, joints, use_rigid_transforms=False):\n \"\"\" \n Computes the forward kinematics of all links and the end-effector\n\n Parameters\n ----------\n joints : :obj:`list` \n A list of 7 numbers that correspond to to the joint angles in radians\n use_rigid_transforms : :obj:`bool` \n Optional: Defaults to False.\n If True, converts result to RigidTransform objects. This is slower.\n\n Returns\n -------\n transforms : :obj:`list` \n A list of 9 RigidTransforms or ndarrays in panda_link0 to panda_link7, \n the franka_tool_base, and franka_tool frames.\n \n \"\"\"\n transforms_matrices = np.repeat(np.expand_dims(np.eye(4), 0), len(FC.DH_PARAMS) + 3, axis=0)\n prev_transform = np.eye(4)\n\n for i in range(len(FC.DH_PARAMS)):\n a, d, alpha, theta = FC.DH_PARAMS[i]\n\n if i < FC.N_REV_JOINTS:\n theta = theta + joints[i]\n\n ca, sa = np.cos(alpha), np.sin(alpha)\n ct, st = np.cos(theta), np.sin(theta)\n self._dh_alpha_rot[1, 1] = ca\n self._dh_alpha_rot[1, 2] = -sa\n self._dh_alpha_rot[2, 1] = sa\n self._dh_alpha_rot[2, 2] = ca\n\n self._dh_a_trans[0, 3] = a\n self._dh_d_trans[2, 3] = d\n\n self._dh_theta_rot[0, 0] = ct\n self._dh_theta_rot[0, 1] = -st\n self._dh_theta_rot[1, 0] = st\n self._dh_theta_rot[1, 1] = ct\n\n delta_transform_matrix = self._dh_alpha_rot @ self._dh_a_trans @ self._dh_d_trans @ self._dh_theta_rot\n\n transforms_matrices[i + 1] = prev_transform @ delta_transform_matrix\n prev_transform = transforms_matrices[i + 1]\n\n transforms_matrices[10] = transforms_matrices[9]\n transforms_matrices[11] = transforms_matrices[10] @ self._tool_delta_pose.matrix\n\n rigid_transforms = []\n if use_rigid_transforms:\n for i in range(8):\n rigid_transforms.append(\n RigidTransform(rotation=transforms_matrices[i, :3, :3], translation=transforms_matrices[i, :3, 3],\n from_frame='panda_link{}'.format(i + 1), to_frame='world'\n ))\n\n rigid_transforms.append(\n RigidTransform(rotation=transforms_matrices[8, :3, :3], translation=transforms_matrices[8, :3, 3],\n from_frame='panda_hand', to_frame='world'\n ))\n\n transform_tool_base = rigid_transforms[-1].as_frames(from_frame='franka_tool_base')\n transform_tool = transform_tool_base * self._tool_delta_pose\n\n rigid_transforms.append(transform_tool_base)\n rigid_transforms.append(transform_tool)\n\n return rigid_transforms\n else:\n return transforms_matrices\n\n def get_jacobian(self, joints):\n \"\"\" \n Computes the geometric jacobian\n \n Parameters\n ----------\n joints : :obj:`list` \n A list of 7 numbers that correspond to the joint angles in radians.\n\n Returns\n -------\n jacobian : :obj:`numpy.ndarray`\n A 6 by 7 jacobian matrix.\n \"\"\"\n transforms = self.get_links_transforms(joints, use_rigid_transforms=False)\n\n joints_pos = transforms[1:FC.N_REV_JOINTS + 1, :3, 3]\n ee_pos = transforms[-1, :3, 3]\n axes = transforms[1:FC.N_REV_JOINTS + 1, :3, 2]\n\n J = np.r_[np.cross(axes, ee_pos - joints_pos).T, axes.T]\n\n return J\n\n def get_collision_boxes_poses(self, joints=None, use_rigid_transforms=False):\n \"\"\" \n Computes the transforms of all collision boxes in world frame.\n\n Parameters\n ----------\n joints : :obj:`list` \n Optional: Defaults to None\n A list of 7 numbers that correspond to to the joint angles in radians\n If None, will use current real robot joints.\n use_rigid_transforms : :obj:`bool` \n Optional: Defaults to False.\n If True, converts result to RigidTransform objects. This is slower.\n\n Returns\n -------\n transforms : :obj:`list` \n A list of RigidTransforms or ndarrays for all collision boxes in world frame.\n \n \"\"\"\n if joints is None:\n joints = self.get_joints()\n\n fk = self.get_links_transforms(joints, use_rigid_transforms=False)\n\n box_poses = []\n for box_n, link in enumerate(FC.COLLISION_BOX_LINKS):\n link_transform = fk[link]\n box_pose_world = link_transform @ FC.COLLISION_BOX_POSES[box_n]\n box_poses.append(box_pose_world)\n\n if use_rigid_transforms:\n box_transforms = [RigidTransform(\n rotation=box_pose[:3, :3],\n translation=box_pose[:3, 3], \n from_frame='box{}'.format(box_n), to_frame='world'\n ) for box_n, box_pose in enumerate(box_poses)]\n return box_transforms\n else:\n return box_poses\n \n def publish_sensor_values(self, sensor_values=None):\n \"\"\" \n Publish sensor values to franka-interface\n\n Parameters\n ----------\n sensor_values : :obj:`SensorDataGroup` \n A SensorDataGroup message.\n \"\"\"\n if sensor_values is None:\n return False\n \n self._sensor_pub.publish(sensor_values)\n\n def publish_joints(self, joints=None):\n \"\"\" \n Publish the Franka joints to ROS\n\n Parameters\n ----------\n joints : :obj:`list` \n Optional: Defaults to None\n A list of 7 numbers that correspond to to the joint angles in radians\n If None, will use current real robot joints.\n \"\"\"\n if joints is None:\n joints = self.get_joints()\n \n joint_state = JointState()\n joint_state.name = FC.JOINT_NAMES\n joint_state.header.stamp = rospy.Time.now()\n\n if len(joints) == 7:\n joints = np.concatenate([joints, [0, 0]])\n joint_state.position = joints\n\n self._joint_state_pub.publish(joint_state)\n\n def publish_collision_boxes(self, joints=None):\n \"\"\" \n Publish the Franka collsion boxes to ROS\n\n Parameters\n ----------\n joints : :obj:`list` \n Optional: Defaults to None\n A list of 7 numbers that correspond to to the joint angles in radians.\n If None, will use current real robot joints.\n \"\"\"\n if joints is None:\n joints = self.get_joints()\n\n box_poses_world = self.get_collision_boxes_poses(joints)\n\n for i, pose in enumerate(box_poses_world):\n self._collision_boxes_data[i, :3] = pose[:3, 3]\n q = quaternion.from_rotation_matrix(pose[:3, :3])\n\n for j, k in enumerate('wxyz'):\n self._collision_boxes_data[i, 3 + j] = getattr(q, k)\n\n self._collision_boxes_pub.publish_boxes(self._collision_boxes_data)\n\n def check_box_collision(self, box, joints=None):\n \"\"\" \n Checks if the given joint configuration is in collision with a box\n\n Parameters\n ----------\n joints : :obj:`list` \n Optional: Defaults to None\n A list of 7 numbers that correspond to to the joint angles in radians\n If None, will use current real robot joints.\n\n box : :obj:`list` \n The position of the center of the box [x, y, z, r, p, y] and the \n length, width, and height [l, w, h].\n\n Returns\n -------\n in_collision : :obj:`bool`\n Flag that indicates whether the robot would be in collision. \n \"\"\"\n box_pos, box_rpy, box_hsizes = box[:3], box[3:6], box[6:]/2\n box_q = quaternion.from_euler_angles(box_rpy)\n box_axes = quaternion.as_rotation_matrix(box_q)\n\n self._box_vertices_offset[:,:] = self._vertex_offset_signs * box_hsizes\n box_vertices = (box_axes @ self._box_vertices_offset.T + np.expand_dims(box_pos, 1)).T\n\n box_hdiag = np.linalg.norm(box_hsizes)\n min_col_dists = box_hdiag + self._collision_box_hdiags\n\n franka_box_poses = self.get_collision_boxes_poses(joints)\n for i, franka_box_pose in enumerate(franka_box_poses):\n fbox_pos = franka_box_pose[:3, 3]\n fbox_axes = franka_box_pose[:3, :3]\n\n # coarse collision check\n if np.linalg.norm(fbox_pos - box_pos) > min_col_dists[i]:\n continue\n\n fbox_vertex_offsets = self._collision_box_vertices_offset[i]\n fbox_vertices = fbox_vertex_offsets @ fbox_axes.T + fbox_pos\n\n # construct axes\n cross_product_pairs = np.array(list(product(box_axes.T, fbox_axes.T)))\n cross_axes = np.cross(cross_product_pairs[:,0], cross_product_pairs[:,1]).T\n self._collision_proj_axes[:, :3] = box_axes\n self._collision_proj_axes[:, 3:6] = fbox_axes\n self._collision_proj_axes[:, 6:] = cross_axes\n\n # projection\n box_projs = box_vertices @ self._collision_proj_axes\n fbox_projs = fbox_vertices @ self._collision_proj_axes\n min_box_projs, max_box_projs = box_projs.min(axis=0), box_projs.max(axis=0)\n min_fbox_projs, max_fbox_projs = fbox_projs.min(axis=0), fbox_projs.max(axis=0)\n\n # check if no separating planes exist\n if np.all([min_box_projs <= max_fbox_projs, max_box_projs >= min_fbox_projs]):\n return True\n \n return False\n\n def is_joints_in_collision_with_boxes(self, joints=None, boxes=None):\n \"\"\" \n Checks if the given joint configuration is in collision with boxes\n or the workspace walls.\n\n Parameters\n ----------\n joints : :obj:`list` \n Optional: Defaults to None\n A list of 7 numbers that correspond to the joint angles in radians\n If None, will use current real robot joints.\n\n boxes : :obj:`list` \n List of boxes where each box is a list of 9 floats that contains \n the position of the center of the box [x, y, z, r, p, y] and the \n length, width, and height [l, w, h].\n\n Returns\n -------\n in_collision : :obj:`bool`\n Flag that indicates whether the robot would be in collision. \n \"\"\"\n if boxes is None:\n boxes = FC.WORKSPACE_WALLS\n\n for box in boxes:\n if self.check_box_collision(box, joints=joints):\n return True\n\n return False\n\n \"\"\"\n Misc\n \"\"\"\n def reset_joints(self, duration=5, block=True, ignore_errors=True, skill_desc=''):\n \"\"\"\n Commands Arm to go to hardcoded home joint configuration\n\n Parameters\n ----------\n duration : :obj:`float`\n How much time this robot motion should take.\n block : :obj:`bool`\n Function blocks by default. If False, the function becomes\n asynchronous and can be preempted.\n ignore_errors : :obj:`bool`\n Function ignores errors by default. If False, errors and some \n exceptions can be thrown.\n skill_desc : :obj:`str` \n Skill description to use for logging on control-pc.\n \"\"\"\n self.goto_joints(FC.HOME_JOINTS, duration=duration, skill_desc=skill_desc, block=block, ignore_errors=ignore_errors)\n\n def reset_pose(self, duration=5, block=True, ignore_errors=True, skill_desc=''):\n \"\"\"\n Commands Arm to go to hardcoded home pose\n\n Parameters\n ----------\n duration : :obj:`float`\n How much time this robot motion should take.\n block : :obj:`bool`\n Function blocks by default. If False, the function becomes\n asynchronous and can be preempted.\n ignore_errors : :obj:`bool`\n Function ignores errors by default. If False, errors and some \n exceptions can be thrown.\n skill_desc : :obj:`str` \n Skill description to use for logging on control-pc.\n \"\"\"\n self.goto_pose(FC.HOME_POSE, duration=duration, skill_desc=skill_desc, block=block, ignore_errors=ignore_errors)\n\n def is_joints_reachable(self, joints):\n \"\"\"\n Checks if the given joint configuration is within joint limits.\n\n Parameters\n ----------\n joints : :obj:`list` \n A list of 7 numbers that correspond to the joint angles in radians.\n\n Returns\n -------\n joints_reachable : :obj:`bool`\n Flag that is True if all joints are within joint limits.\n \"\"\"\n for i, val in enumerate(joints):\n if val <= FC.JOINT_LIMITS_MIN[i] or val >= FC.JOINT_LIMITS_MAX[i]:\n return False\n\n return True\n\n \"\"\"\n Unimplemented\n \"\"\"\n\n def apply_joint_torques(self, torques, duration, ignore_errors=True, skill_desc='',):\n \"\"\"\n NOT IMPLEMENTED YET\n\n Commands the arm to apply given joint torques for duration seconds.\n\n Parameters\n ----------\n torques : :obj:`list` \n A list of 7 numbers that correspond to torques in Nm.\n duration : :obj:`float`\n How much time this robot motion should take.\n skill_desc : :obj:`str` \n Skill description to use for logging on control-pc.\n \"\"\"\n pass\n\n def set_speed(self, speed):\n \"\"\"\n NOT IMPLEMENTED YET\n\n Sets the current target speed parameter\n\n Parameters\n ----------\n speed : :obj:`float`\n Current target speed parameter\n \"\"\"\n pass\n\n def get_speed(self, speed):\n \"\"\"\n NOT IMPLEMENTED YET\n\n Returns the current target speed parameter\n\n Returns\n -------\n speed : :obj:`float`\n Current target speed parameter\n \"\"\"\n pass\n" }, { "path": "frankapy/franka_arm_state_client.py", "content": "import logging\n\nimport numpy as np\nimport rospy\nfrom franka_interface_msgs.srv import GetCurrentRobotStateCmd\n\nfrom .utils import franka_pose_to_rigid_transform\n\n\nclass FrankaArmStateClient:\n\n def __init__(self, new_ros_node=True, robot_state_server_name='/get_current_robot_state_server_node_1/get_current_robot_state_server', offline=False):\n if new_ros_node:\n rospy.init_node('FrankaArmStateClient', anonymous=True)\n\n self._offline = offline\n if not self._offline:\n rospy.wait_for_service(robot_state_server_name)\n self._get_current_robot_state = rospy.ServiceProxy(robot_state_server_name, GetCurrentRobotStateCmd)\n\n def get_data(self):\n '''Get all fields of current robot data in a dict.\n\n Returns:\n dict of robot state\n '''\n if self._offline:\n logging.warn('In offline mode - FrankaArmStateClient will return 0 values.')\n return {\n 'cartesian_velocities': np.zeros(6),\n 'pose': franka_pose_to_rigid_transform(np.eye(4)),\n 'pose_desired': franka_pose_to_rigid_transform(np.eye(4)),\n 'joint_torques': np.zeros(7),\n 'joint_torques_derivative': np.zeros(7),\n 'joints': np.zeros(7),\n 'joints_desired': np.zeros(7),\n 'joint_velocities': np.zeros(7),\n 'gripper_width': 0,\n 'gripper_is_grasped': False,\n 'ee_force_torque': np.zeros(6)\n }\n\n ros_data = self._get_current_robot_state().robot_state\n\n data = {\n 'cartesian_velocities': np.array(ros_data.O_dP_EE_c),\n 'pose': franka_pose_to_rigid_transform(ros_data.O_T_EE),\n 'pose_desired': franka_pose_to_rigid_transform(ros_data.O_T_EE_d),\n 'joint_torques': np.array(ros_data.tau_J),\n 'joint_torques_derivative': np.array(ros_data.dtau_J),\n 'joints': np.array(ros_data.q),\n 'joints_desired': np.array(ros_data.q_d),\n 'joint_velocities': np.array(ros_data.dq),\n 'gripper_width': ros_data.gripper_width,\n 'gripper_is_grasped': ros_data.gripper_is_grasped,\n 'ee_force_torque': np.array(ros_data.O_F_ext_hat_K)\n }\n\n return data\n\n def get_pose(self):\n '''Get the current pose.\n\n Returns:\n RigidTransform\n '''\n return self.get_data()['pose']\n\n def get_joints(self):\n '''Get the current joint configuration.\n\n Returns:\n ndarray of shape (7,)\n '''\n return self.get_data()['joints']\n\n def get_joint_torques(self):\n '''Get the current joint torques.\n\n Returns:\n ndarray of shape (7,)\n '''\n return self.get_data()['joint_torques']\n\n def get_joint_velocities(self):\n '''Get the current joint velocities.\n\n Returns:\n ndarray of shape (7,)\n '''\n return self.get_data()['joint_velocities']\n\n def get_gripper_width(self):\n '''Get most recent gripper width. Note this value will *not* be updated\n during a control command.\n\n Returns:\n float of gripper width in meters\n '''\n return self.get_data()['gripper_width']\n\n def get_gripper_is_grasped(self):\n '''Returns whether or not the gripper is grasping something. Note this\n value will *not* be updated during a control command.\n\n Returns:\n True if gripper is grasping something. False otherwise\n '''\n return self.get_data()['gripper_is_grasped']\n\n def get_ee_force_torque(self):\n '''Get the current ee force torque in base frame.\n\n Returns:\n ndarray of shape (6,)\n '''\n\n return self.get_data()['ee_force_torque']" }, { "path": "frankapy/franka_constants.py", "content": "import logging\nimport math\nimport numpy as np\nfrom autolab_core import RigidTransform\n\nclass FrankaConstants:\n '''\n Contains default robot values, as well as robot limits.\n All units are in SI. \n '''\n\n LOGGING_LEVEL = logging.INFO\n\n EMPTY_SENSOR_VALUES = [0]\n\n # translational stiffness, rotational stiffness\n DEFAULT_FORCE_AXIS_TRANSLATIONAL_STIFFNESS = 600\n DEFAULT_FORCE_AXIS_ROTATIONAL_STIFFNESS = 20\n\n # buffer time\n DEFAULT_TERM_BUFFER_TIME = 0.2\n\n HOME_JOINTS = [0, -math.pi / 4, 0, -3 * math.pi / 4, 0, math.pi / 2, math.pi / 4]\n HOME_POSE = RigidTransform(rotation=np.array([\n [1, 0, 0],\n [0, -1, 0],\n [0, 0, -1],\n ]), translation=np.array([0.3069, 0, 0.4867]),\n from_frame='franka_tool', to_frame='world')\n READY_JOINTS = [0, -math.pi/4, 0, -2.85496998, 0, 2.09382820, math.pi/4]\n READY_POSE = RigidTransform(rotation=np.array([\n [1, 0, 0],\n [0, -1, 0],\n [0, 0, -1],\n ]), translation=np.array([0.3069, 0, 0.2867]),\n from_frame='franka_tool', to_frame='world')\n\n # See https://frankaemika.github.io/docs/control_parameters.html\n JOINT_LIMITS_MIN = [-2.8973, -1.7628, -2.8973, -3.0718, -2.8973, -0.0175, -2.8973]\n JOINT_LIMITS_MAX = [2.8973, 1.7628, 2.8973, -0.0698, 2.8973, 3.7525, 2.8973]\n\n DEFAULT_POSE_THRESHOLDS = [0.0001, 0.0001, 0.0001, 0.0001, 0.0001, 0.0001]\n DEFAULT_JOINT_THRESHOLDS = [0.0001, 0.0001, 0.0001, 0.0001, 0.0001, 0.0001, 0.0001]\n\n GRIPPER_WIDTH_MAX = 0.08\n GRIPPER_WIDTH_MIN = 0\n GRIPPER_MAX_FORCE = 60\n\n MAX_LIN_MOMENTUM = 20\n MAX_ANG_MOMENTUM = 2\n MAX_LIN_MOMENTUM_CONSTRAINED = 100\n\n DEFAULT_FRANKA_INTERFACE_TIMEOUT = 10\n ACTION_WAIT_LOOP_TIME = 0.001\n\n GRIPPER_CMD_SLEEP_TIME = 0.2\n\n DEFAULT_K_GAINS = [600.0, 600.0, 600.0, 600.0, 250.0, 150.0, 50.0]\n DEFAULT_D_GAINS = [50.0, 50.0, 50.0, 50.0, 30.0, 25.0, 15.0]\n DEFAULT_TRANSLATIONAL_STIFFNESSES = [600.0, 600.0, 600.0]\n DEFAULT_ROTATIONAL_STIFFNESSES = [50.0, 50.0, 50.0]\n\n DEFAULT_JOINT_IMPEDANCES = [3000, 3000, 3000, 2500, 2500, 2000, 2000]\n DEFAULT_CARTESIAN_IMPEDANCES = [3000, 3000, 3000, 300, 300, 300]\n\n DEFAULT_LOWER_TORQUE_THRESHOLDS_ACCEL = [20.0,20.0,18.0,18.0,16.0,14.0,12.0]\n DEFAULT_UPPER_TORQUE_THRESHOLDS_ACCEL = [120.0,120.0,120.0,118.0,116.0,114.0,112.0]\n DEFAULT_LOWER_TORQUE_THRESHOLDS_NOMINAL = [20.0,20.0,18.0,18.0,16.0,14.0,12.0]\n DEFAULT_UPPER_TORQUE_THRESHOLDS_NOMINAL = [120.0,120.0,118.0,118.0,116.0,114.0,112.0]\n\n DEFAULT_LOWER_FORCE_THRESHOLDS_ACCEL = [10.0,10.0,10.0,10.0,10.0,10.0]\n DEFAULT_UPPER_FORCE_THRESHOLDS_ACCEL = [120.0,120.0,120.0,125.0,125.0,125.0]\n DEFAULT_LOWER_FORCE_THRESHOLDS_NOMINAL = [10.0,10.0,10.0,10.0,10.0,10.0]\n DEFAULT_UPPER_FORCE_THRESHOLDS_NOMINAL = [120.0,120.0,120.0,125.0,125.0,125.0]\n\n DH_PARAMS = np.array([[0, 0.333, 0, 0],\n [0, 0, -np.pi/2, 0],\n [0, 0.316, np.pi/2, 0],\n [0.0825, 0, np.pi/2, 0],\n [-0.0825, 0.384, -np.pi/2, 0],\n [0, 0, np.pi/2, 0],\n [0.088, 0, np.pi/2, 0],\n [0, 0.107, 0, 0],\n [0, 0.1034, 0, 0]])\n\n N_REV_JOINTS = 7\n\n COLLISION_BOX_SHAPES = np.array([\n [0.23, 0.2, 0.1],\n [0.13, 0.12, 0.1], \n [0.12, 0.1, 0.2],\n [0.15, 0.27, 0.11],\n [0.12, 0.1, 0.2],\n [0.13, 0.12, 0.25],\n [0.13, 0.23, 0.15],\n [0.12, 0.12, 0.4],\n [0.12, 0.12, 0.25],\n [0.13, 0.23, 0.12],\n [0.12, 0.12, 0.2],\n [0.08, 0.22, 0.17]\n ])\n\n COLLISION_BOX_LINKS = [1, 1, 1, 1, 1, 3, 4, 5, 5, 5, 7, 7]\n\n COLLISION_BOX_POSES = np.array([\n [[ 1. , 0. , 0. , -0.04 ],\n [ 0. , 1. , 0. , 0. ],\n [ 0. , 0. , 1. , -0.283 ],\n [ 0. , 0. , 0. , 1. ]],\n [[ 1. , 0. , 0. , -0.009 ],\n [ 0. , 1. , 0. , 0. ],\n [ 0. , 0. , 1. , -0.183 ],\n [ 0. , 0. , 0. , 1. ]],\n [[ 1. , 0. , 0. , 0. ],\n [ 0. , 0.81038486, -0.58589793, -0.032 ],\n [ 0. , 0.58589793, 0.81038486, -0.082 ],\n [ 0. , 0. , 0. , 1. ]],\n [[ 1. , 0. , 0. , -0.008 ],\n [ 0. , 1. , 0. , 0. ],\n [ 0. , 0. , 1. , 0. ],\n [ 0. , 0. , 0. , 1. ]],\n [[ 1. , 0. , 0. , 0. ],\n [ 0. , 0.81038486, -0.58589793, 0.042 ],\n [ 0. , 0.58589793, 0.81038486, 0.067 ],\n [ 0. , 0. , 0. , 1. ]],\n [[ 1. , 0. , 0. , 0.00687 ],\n [ 0. , 1. , 0. , 0. ],\n [ 0. , 0. , 1. , -0.139 ],\n [ 0. , 0. , 0. , 1. ]],\n [[ 1. , -0. , 0. , -0.008 ],\n [ 0. , 0. , 1. , 0.004 ],\n [-0. , -1. , 0. , 0. ],\n [ 0. , 0. , 0. , 1. ]],\n [[ 1. , -0. , 0. , 0.00422 ],\n [ 0. , 0.98480775, 0.17364817, 0.05367 ],\n [-0. , -0.17364817, 0.98480775, -0.121 ],\n [ 0. , 0. , 0. , 1. ]],\n [[ 1. , 0. , 0. , 0.00422 ],\n [ 0. , 1. , 0. , 0.00367 ],\n [ 0. , 0. , 1. , -0.263 ],\n [ 0. , 0. , 0. , 1. ]],\n [[ 1. , 0. , 0. , 0.00328 ],\n [ 0. , 1. , 0. , 0.0176 ],\n [ 0. , 0. , 1. , -0.0055 ],\n [ 0. , 0. , 0. , 1. ]],\n [[ 1. , 0. , 0. , -0.0136 ],\n [ 0. , -1. , 0. , 0.0092 ],\n [ 0. , 0. , -1. , 0.0083 ],\n [ 0. , 0. , 0. , 1. ]],\n [[ 0.70710678, 0.70710678, 0. , 0.0136 ],\n [-0.70710678, 0.70710678, -0. , -0.0092 ],\n [-0. , 0. , 1. , 0.1457 ],\n [ 0. , 0. , 0. , 1. ]]]\n )\n\n JOINT_NAMES = ['panda_joint1',\n 'panda_joint2',\n 'panda_joint3',\n 'panda_joint4',\n 'panda_joint5',\n 'panda_joint6',\n 'panda_joint7',\n 'panda_finger_joint1', \n 'panda_finger_joint2']\n\n WORKSPACE_WALLS = np.array([\n # sides\n [0.15, 0.46, 0.5, 0, 0, 0, 1.2, 0.01, 1.1],\n [0.15, -0.46, 0.5, 0, 0, 0, 1.2, 0.01, 1.1],\n # back\n [-0.41, 0, 0.5, 0, 0, 0, 0.01, 1, 1.1],\n # front\n [0.75, 0, 0.5, 0, 0, 0, 0.01, 1, 1.1],\n # top\n [0.2, 0, 1, 0, 0, 0, 1.2, 1, 0.01],\n # bottom\n [0.2, 0, -0.05, 0, 0, 0, 1.2, 1, 0.01]\n ])\n\n DEFAULT_SENSOR_PUBLISHER_TOPIC = 'franka_ros_interface/sensor'\n DYNAMIC_SKILL_WAIT_TIME = 0.3\n\n DEFAULT_HFPC_FORCE_GAIN = [0.1] * 6\n DEFAULT_HFPC_S = [1, 1, 1, 1, 1, 1]" }, { "path": "frankapy/franka_interface_common_definitions.py", "content": "#######################################################################\n# #\n# Important: Any Changes here should also be reflected in changes #\n# in the franka-interface-common definitions.h file as well. #\n# #\n# The order of the enums matter!! #\n# #\n####################################################################### \n\n\n_ENUM_COUNTER = {}\ndef _enum_auto(key):\n if key not in _ENUM_COUNTER:\n _ENUM_COUNTER[key] = 0\n val = _ENUM_COUNTER[key]\n _ENUM_COUNTER[key] += 1\n return val\n\n\nclass SkillType: \n CartesianPoseSkill = _enum_auto('SkillType')\n CartesianVelocitySkill = _enum_auto('SkillType')\n ForceTorqueSkill = _enum_auto('SkillType')\n GripperSkill = _enum_auto('SkillType')\n ImpedanceControlSkill = _enum_auto('SkillType')\n JointPositionSkill = _enum_auto('SkillType')\n JointVelocitySkill = _enum_auto('SkillType')\n\n\nclass MetaSkillType:\n BaseMetaSkill = _enum_auto('MetaSkillType')\n JointPositionContinuousSkill = _enum_auto('MetaSkillType')\n\n\nclass TrajectoryGeneratorType:\n CartesianVelocityTrajectoryGenerator = _enum_auto('TrajectoryGeneratorType')\n CubicHermiteSplineJointTrajectoryGenerator = _enum_auto('TrajectoryGeneratorType')\n CubicHermiteSplinePoseTrajectoryGenerator = _enum_auto('TrajectoryGeneratorType')\n GoalPoseDmpTrajectoryGenerator = _enum_auto('TrajectoryGeneratorType')\n GripperTrajectoryGenerator = _enum_auto('TrajectoryGeneratorType')\n ImpulseTrajectoryGenerator = _enum_auto('TrajectoryGeneratorType')\n JointDmpTrajectoryGenerator = _enum_auto('TrajectoryGeneratorType')\n JointVelocityTrajectoryGenerator = _enum_auto('TrajectoryGeneratorType')\n LinearForcePositionTrajectoryGenerator = _enum_auto('TrajectoryGeneratorType')\n LinearJointTrajectoryGenerator = _enum_auto('TrajectoryGeneratorType')\n LinearPoseTrajectoryGenerator = _enum_auto('TrajectoryGeneratorType')\n MinJerkJointTrajectoryGenerator = _enum_auto('TrajectoryGeneratorType')\n MinJerkPoseTrajectoryGenerator = _enum_auto('TrajectoryGeneratorType')\n PassThroughCartesianVelocityTrajectoryGenerator = _enum_auto('TrajectoryGeneratorType')\n PassThroughForcePositionTrajectoryGenerator = _enum_auto('TrajectoryGeneratorType')\n PassThroughJointTrajectoryGenerator = _enum_auto('TrajectoryGeneratorType')\n PassThroughJointVelocityTrajectoryGenerator = _enum_auto('TrajectoryGeneratorType')\n PassThroughPoseTrajectoryGenerator = _enum_auto('TrajectoryGeneratorType')\n PoseDmpTrajectoryGenerator = _enum_auto('TrajectoryGeneratorType')\n QuaternionPoseDmpTrajectoryGenerator = _enum_auto('TrajectoryGeneratorType')\n RelativeLinearPoseTrajectoryGenerator = _enum_auto('TrajectoryGeneratorType')\n RelativeMinJerkPoseTrajectoryGenerator = _enum_auto('TrajectoryGeneratorType')\n SineJointTrajectoryGenerator = _enum_auto('TrajectoryGeneratorType')\n SinePoseTrajectoryGenerator = _enum_auto('TrajectoryGeneratorType')\n StayInInitialJointsTrajectoryGenerator = _enum_auto('TrajectoryGeneratorType')\n StayInInitialPoseTrajectoryGenerator = _enum_auto('TrajectoryGeneratorType')\n\n\nclass FeedbackControllerType:\n CartesianImpedanceFeedbackController = _enum_auto('FeedbackControllerType')\n EECartesianImpedanceFeedbackController = _enum_auto('FeedbackControllerType')\n ForceAxisImpedenceFeedbackController = _enum_auto('FeedbackControllerType')\n ForcePositionFeedbackController = _enum_auto('FeedbackControllerType')\n JointImpedanceFeedbackController = _enum_auto('FeedbackControllerType')\n NoopFeedbackController = _enum_auto('FeedbackControllerType')\n PassThroughFeedbackController = _enum_auto('FeedbackControllerType')\n PassThroughJointTorqueFeedbackController = _enum_auto('FeedbackControllerType')\n SetInternalImpedanceFeedbackController = _enum_auto('FeedbackControllerType')\n\n\nclass TerminationHandlerType:\n ContactTerminationHandler = _enum_auto('TerminationHandlerType')\n FinalJointTerminationHandler = _enum_auto('TerminationHandlerType')\n FinalPoseTerminationHandler = _enum_auto('TerminationHandlerType')\n NoopTerminationHandler = _enum_auto('TerminationHandlerType')\n TimeTerminationHandler = _enum_auto('TerminationHandlerType')\n\n\nclass SkillStatus:\n TO_START = _enum_auto('SkillStatus')\n RUNNING = _enum_auto('SkillStatus')\n FINISHED = _enum_auto('SkillStatus')\n VIRT_COLL_ERR = _enum_auto('SkillStatus')\n\n\nclass SensorDataMessageType:\n BOUNDING_BOX = _enum_auto('SensorDataMessageType')\n CARTESIAN_IMPEDANCE = _enum_auto('SensorDataMessageType')\n CARTESIAN_VELOCITY = _enum_auto('SensorDataMessageType')\n FORCE_POSITION = _enum_auto('SensorDataMessageType')\n FORCE_POSITION_GAINS = _enum_auto('SensorDataMessageType')\n JOINT_POSITION_VELOCITY = _enum_auto('SensorDataMessageType')\n JOINT_POSITION = _enum_auto('SensorDataMessageType')\n JOINT_VELOCITY = _enum_auto('SensorDataMessageType')\n JOINT_TORQUE = _enum_auto('SensorDataMessageType')\n POSE_POSITION_VELOCITY = _enum_auto('SensorDataMessageType')\n POSE_POSITION = _enum_auto('SensorDataMessageType')\n SHOULD_TERMINATE = _enum_auto('SensorDataMessageType')\n" }, { "path": "frankapy/proto/__init__.py", "content": "from .feedback_controller_params_msg_pb2 import *\nfrom .sensor_msg_pb2 import *\nfrom .termination_handler_params_msg_pb2 import *\nfrom .trajectory_generator_params_msg_pb2 import *" }, { "path": "frankapy/proto/feedback_controller_params_msg.proto", "content": "syntax = \"proto2\";\n\nmessage CartesianImpedanceFeedbackControllerMessage {\n repeated double translational_stiffnesses = 1;\n repeated double rotational_stiffnesses = 2;\n}\n\nmessage ForceAxisFeedbackControllerMessage {\n required double translational_stiffness = 1;\n required double rotational_stiffness = 2;\n\n repeated double axis = 3;\n}\n\nmessage JointImpedanceFeedbackControllerMessage {\n repeated double k_gains = 1;\n repeated double d_gains = 2;\n}\n\nmessage InternalImpedanceFeedbackControllerMessage {\n repeated double cartesian_impedances = 1;\n repeated double joint_impedances = 2;\n}\n\nmessage ForcePositionFeedbackControllerMessage {\n repeated double position_kps_cart = 1;\n repeated double force_kps_cart = 2;\n repeated double position_kps_joint = 3;\n repeated double force_kps_joint = 4;\n repeated double selection = 5;\n required bool use_cartesian_gains = 6;\n}\n\nmessage JointTorqueFeedbackControllerMessage {\n repeated double selection = 1;\n repeated double remove_gravity = 2;\n repeated double joint_torques = 3;\n repeated double k_gains = 4;\n repeated double d_gains = 5;\n}\n" }, { "path": "frankapy/proto/feedback_controller_params_msg_pb2.py", "content": "# -*- coding: utf-8 -*-\n# Generated by the protocol buffer compiler. DO NOT EDIT!\n# source: feedback_controller_params_msg.proto\n\"\"\"Generated protocol buffer code.\"\"\"\nfrom google.protobuf.internal import builder as _builder\nfrom google.protobuf import descriptor as _descriptor\nfrom google.protobuf import descriptor_pool as _descriptor_pool\nfrom google.protobuf import symbol_database as _symbol_database\n# @@protoc_insertion_point(imports)\n\n_sym_db = _symbol_database.Default()\n\n\n\n\nDESCRIPTOR = _descriptor_pool.Default().AddSerializedFile(b'\\n$feedback_controller_params_msg.proto\\\"p\\n+CartesianImpedanceFeedbackControllerMessage\\x12!\\n\\x19translational_stiffnesses\\x18\\x01 \\x03(\\x01\\x12\\x1e\\n\\x16rotational_stiffnesses\\x18\\x02 \\x03(\\x01\\\"q\\n\\\"ForceAxisFeedbackControllerMessage\\x12\\x1f\\n\\x17translational_stiffness\\x18\\x01 \\x02(\\x01\\x12\\x1c\\n\\x14rotational_stiffness\\x18\\x02 \\x02(\\x01\\x12\\x0c\\n\\x04\\x61xis\\x18\\x03 \\x03(\\x01\\\"K\\n\\'JointImpedanceFeedbackControllerMessage\\x12\\x0f\\n\\x07k_gains\\x18\\x01 \\x03(\\x01\\x12\\x0f\\n\\x07\\x64_gains\\x18\\x02 \\x03(\\x01\\\"d\\n*InternalImpedanceFeedbackControllerMessage\\x12\\x1c\\n\\x14\\x63\\x61rtesian_impedances\\x18\\x01 \\x03(\\x01\\x12\\x18\\n\\x10joint_impedances\\x18\\x02 \\x03(\\x01\\\"\\xc0\\x01\\n&ForcePositionFeedbackControllerMessage\\x12\\x19\\n\\x11position_kps_cart\\x18\\x01 \\x03(\\x01\\x12\\x16\\n\\x0e\\x66orce_kps_cart\\x18\\x02 \\x03(\\x01\\x12\\x1a\\n\\x12position_kps_joint\\x18\\x03 \\x03(\\x01\\x12\\x17\\n\\x0f\\x66orce_kps_joint\\x18\\x04 \\x03(\\x01\\x12\\x11\\n\\tselection\\x18\\x05 \\x03(\\x01\\x12\\x1b\\n\\x13use_cartesian_gains\\x18\\x06 \\x02(\\x08\\\"\\x8a\\x01\\n$JointTorqueFeedbackControllerMessage\\x12\\x11\\n\\tselection\\x18\\x01 \\x03(\\x01\\x12\\x16\\n\\x0eremove_gravity\\x18\\x02 \\x03(\\x01\\x12\\x15\\n\\rjoint_torques\\x18\\x03 \\x03(\\x01\\x12\\x0f\\n\\x07k_gains\\x18\\x04 \\x03(\\x01\\x12\\x0f\\n\\x07\\x64_gains\\x18\\x05 \\x03(\\x01')\n\n_builder.BuildMessageAndEnumDescriptors(DESCRIPTOR, globals())\n_builder.BuildTopDescriptorsAndMessages(DESCRIPTOR, 'feedback_controller_params_msg_pb2', globals())\nif _descriptor._USE_C_DESCRIPTORS == False:\n\n DESCRIPTOR._options = None\n _CARTESIANIMPEDANCEFEEDBACKCONTROLLERMESSAGE._serialized_start=40\n _CARTESIANIMPEDANCEFEEDBACKCONTROLLERMESSAGE._serialized_end=152\n _FORCEAXISFEEDBACKCONTROLLERMESSAGE._serialized_start=154\n _FORCEAXISFEEDBACKCONTROLLERMESSAGE._serialized_end=267\n _JOINTIMPEDANCEFEEDBACKCONTROLLERMESSAGE._serialized_start=269\n _JOINTIMPEDANCEFEEDBACKCONTROLLERMESSAGE._serialized_end=344\n _INTERNALIMPEDANCEFEEDBACKCONTROLLERMESSAGE._serialized_start=346\n _INTERNALIMPEDANCEFEEDBACKCONTROLLERMESSAGE._serialized_end=446\n _FORCEPOSITIONFEEDBACKCONTROLLERMESSAGE._serialized_start=449\n _FORCEPOSITIONFEEDBACKCONTROLLERMESSAGE._serialized_end=641\n _JOINTTORQUEFEEDBACKCONTROLLERMESSAGE._serialized_start=644\n _JOINTTORQUEFEEDBACKCONTROLLERMESSAGE._serialized_end=782\n# @@protoc_insertion_point(module_scope)\n" }, { "path": "frankapy/proto/sensor_msg.proto", "content": "// franka-interface/proto/sensor_msg.proto and frankapy/protosensor_msg.proto should match\n\nsyntax = \"proto2\";\n\nmessage BoundingBox {\n required string name = 1;\n required int32 id = 2;\n\n required int32 x = 3;\n required int32 y = 4;\n required int32 w = 5;\n required int32 h = 6;\n}\n\nmessage JointPositionVelocitySensorMessage {\n required int32 id = 1;\n required double timestamp = 2;\n required double seg_run_time = 3;\n\n repeated double joints = 4;\n repeated double joint_vels = 5;\n}\n\nmessage PosePositionVelocitySensorMessage {\n required int32 id = 1;\n required double timestamp = 2;\n required double seg_run_time = 3;\n\n repeated double position = 4;\n repeated double quaternion = 5;\n repeated double pose_velocities = 6;\n}\n\nmessage JointVelocitySensorMessage {\n required int32 id = 1;\n required double timestamp = 2;\n\n repeated double joint_velocities = 3;\n}\n\nmessage JointPositionSensorMessage {\n required int32 id = 1;\n required double timestamp = 2;\n\n repeated double joints = 3;\n}\n\nmessage PosePositionSensorMessage {\n required int32 id = 1;\n required double timestamp = 2;\n\n repeated double position = 3;\n repeated double quaternion = 4;\n}\n\nmessage CartesianVelocitySensorMessage {\n required int32 id = 1;\n required double timestamp = 2;\n\n repeated double cartesian_velocities = 3;\n}\n\nmessage ShouldTerminateSensorMessage {\n required double timestamp = 1;\n required bool should_terminate = 2;\n}\n\nmessage CartesianImpedanceSensorMessage {\n required int32 id = 1;\n required double timestamp = 2;\n\n repeated double translational_stiffnesses = 3;\n repeated double rotational_stiffnesses = 4;\n}\n\nmessage ForcePositionSensorMessage {\n required int32 id = 1;\n required double timestamp = 2;\n required double seg_run_time = 3;\n\n repeated double pose = 4;\n repeated double force = 5;\n}\n\nmessage ForcePositionControllerSensorMessage {\n required int32 id = 1;\n required double timestamp = 2;\n\n repeated double position_kps_cart = 3;\n repeated double force_kps_cart = 4;\n repeated double position_kps_joint = 5;\n repeated double force_kps_joint = 6;\n repeated double selection = 7;\n}\n\nmessage JointTorqueControllerSensorMessage {\n required int32 id = 1;\n required double timestamp = 2;\n\n repeated double selection = 3;\n repeated double remove_gravity = 4;\n repeated double joint_torques = 5;\n repeated double k_gains = 6;\n repeated double d_gains = 7;\n}" }, { "path": "frankapy/proto/sensor_msg_pb2.py", "content": "# -*- coding: utf-8 -*-\n# Generated by the protocol buffer compiler. DO NOT EDIT!\n# source: sensor_msg.proto\n\"\"\"Generated protocol buffer code.\"\"\"\nfrom google.protobuf.internal import builder as _builder\nfrom google.protobuf import descriptor as _descriptor\nfrom google.protobuf import descriptor_pool as _descriptor_pool\nfrom google.protobuf import symbol_database as _symbol_database\n# @@protoc_insertion_point(imports)\n\n_sym_db = _symbol_database.Default()\n\n\n\n\nDESCRIPTOR = _descriptor_pool.Default().AddSerializedFile(b'\\n\\x10sensor_msg.proto\\\"S\\n\\x0b\\x42oundingBox\\x12\\x0c\\n\\x04name\\x18\\x01 \\x02(\\t\\x12\\n\\n\\x02id\\x18\\x02 \\x02(\\x05\\x12\\t\\n\\x01x\\x18\\x03 \\x02(\\x05\\x12\\t\\n\\x01y\\x18\\x04 \\x02(\\x05\\x12\\t\\n\\x01w\\x18\\x05 \\x02(\\x05\\x12\\t\\n\\x01h\\x18\\x06 \\x02(\\x05\\\"}\\n\\\"JointPositionVelocitySensorMessage\\x12\\n\\n\\x02id\\x18\\x01 \\x02(\\x05\\x12\\x11\\n\\ttimestamp\\x18\\x02 \\x02(\\x01\\x12\\x14\\n\\x0cseg_run_time\\x18\\x03 \\x02(\\x01\\x12\\x0e\\n\\x06joints\\x18\\x04 \\x03(\\x01\\x12\\x12\\n\\njoint_vels\\x18\\x05 \\x03(\\x01\\\"\\x97\\x01\\n!PosePositionVelocitySensorMessage\\x12\\n\\n\\x02id\\x18\\x01 \\x02(\\x05\\x12\\x11\\n\\ttimestamp\\x18\\x02 \\x02(\\x01\\x12\\x14\\n\\x0cseg_run_time\\x18\\x03 \\x02(\\x01\\x12\\x10\\n\\x08position\\x18\\x04 \\x03(\\x01\\x12\\x12\\n\\nquaternion\\x18\\x05 \\x03(\\x01\\x12\\x17\\n\\x0fpose_velocities\\x18\\x06 \\x03(\\x01\\\"U\\n\\x1aJointVelocitySensorMessage\\x12\\n\\n\\x02id\\x18\\x01 \\x02(\\x05\\x12\\x11\\n\\ttimestamp\\x18\\x02 \\x02(\\x01\\x12\\x18\\n\\x10joint_velocities\\x18\\x03 \\x03(\\x01\\\"K\\n\\x1aJointPositionSensorMessage\\x12\\n\\n\\x02id\\x18\\x01 \\x02(\\x05\\x12\\x11\\n\\ttimestamp\\x18\\x02 \\x02(\\x01\\x12\\x0e\\n\\x06joints\\x18\\x03 \\x03(\\x01\\\"`\\n\\x19PosePositionSensorMessage\\x12\\n\\n\\x02id\\x18\\x01 \\x02(\\x05\\x12\\x11\\n\\ttimestamp\\x18\\x02 \\x02(\\x01\\x12\\x10\\n\\x08position\\x18\\x03 \\x03(\\x01\\x12\\x12\\n\\nquaternion\\x18\\x04 \\x03(\\x01\\\"]\\n\\x1e\\x43\\x61rtesianVelocitySensorMessage\\x12\\n\\n\\x02id\\x18\\x01 \\x02(\\x05\\x12\\x11\\n\\ttimestamp\\x18\\x02 \\x02(\\x01\\x12\\x1c\\n\\x14\\x63\\x61rtesian_velocities\\x18\\x03 \\x03(\\x01\\\"K\\n\\x1cShouldTerminateSensorMessage\\x12\\x11\\n\\ttimestamp\\x18\\x01 \\x02(\\x01\\x12\\x18\\n\\x10should_terminate\\x18\\x02 \\x02(\\x08\\\"\\x83\\x01\\n\\x1f\\x43\\x61rtesianImpedanceSensorMessage\\x12\\n\\n\\x02id\\x18\\x01 \\x02(\\x05\\x12\\x11\\n\\ttimestamp\\x18\\x02 \\x02(\\x01\\x12!\\n\\x19translational_stiffnesses\\x18\\x03 \\x03(\\x01\\x12\\x1e\\n\\x16rotational_stiffnesses\\x18\\x04 \\x03(\\x01\\\"n\\n\\x1a\\x46orcePositionSensorMessage\\x12\\n\\n\\x02id\\x18\\x01 \\x02(\\x05\\x12\\x11\\n\\ttimestamp\\x18\\x02 \\x02(\\x01\\x12\\x14\\n\\x0cseg_run_time\\x18\\x03 \\x02(\\x01\\x12\\x0c\\n\\x04pose\\x18\\x04 \\x03(\\x01\\x12\\r\\n\\x05\\x66orce\\x18\\x05 \\x03(\\x01\\\"\\xc0\\x01\\n$ForcePositionControllerSensorMessage\\x12\\n\\n\\x02id\\x18\\x01 \\x02(\\x05\\x12\\x11\\n\\ttimestamp\\x18\\x02 \\x02(\\x01\\x12\\x19\\n\\x11position_kps_cart\\x18\\x03 \\x03(\\x01\\x12\\x16\\n\\x0e\\x66orce_kps_cart\\x18\\x04 \\x03(\\x01\\x12\\x1a\\n\\x12position_kps_joint\\x18\\x05 \\x03(\\x01\\x12\\x17\\n\\x0f\\x66orce_kps_joint\\x18\\x06 \\x03(\\x01\\x12\\x11\\n\\tselection\\x18\\x07 \\x03(\\x01\\\"\\xa7\\x01\\n\\\"JointTorqueControllerSensorMessage\\x12\\n\\n\\x02id\\x18\\x01 \\x02(\\x05\\x12\\x11\\n\\ttimestamp\\x18\\x02 \\x02(\\x01\\x12\\x11\\n\\tselection\\x18\\x03 \\x03(\\x01\\x12\\x16\\n\\x0eremove_gravity\\x18\\x04 \\x03(\\x01\\x12\\x15\\n\\rjoint_torques\\x18\\x05 \\x03(\\x01\\x12\\x0f\\n\\x07k_gains\\x18\\x06 \\x03(\\x01\\x12\\x0f\\n\\x07\\x64_gains\\x18\\x07 \\x03(\\x01')\n\n_builder.BuildMessageAndEnumDescriptors(DESCRIPTOR, globals())\n_builder.BuildTopDescriptorsAndMessages(DESCRIPTOR, 'sensor_msg_pb2', globals())\nif _descriptor._USE_C_DESCRIPTORS == False:\n\n DESCRIPTOR._options = None\n _BOUNDINGBOX._serialized_start=20\n _BOUNDINGBOX._serialized_end=103\n _JOINTPOSITIONVELOCITYSENSORMESSAGE._serialized_start=105\n _JOINTPOSITIONVELOCITYSENSORMESSAGE._serialized_end=230\n _POSEPOSITIONVELOCITYSENSORMESSAGE._serialized_start=233\n _POSEPOSITIONVELOCITYSENSORMESSAGE._serialized_end=384\n _JOINTVELOCITYSENSORMESSAGE._serialized_start=386\n _JOINTVELOCITYSENSORMESSAGE._serialized_end=471\n _JOINTPOSITIONSENSORMESSAGE._serialized_start=473\n _JOINTPOSITIONSENSORMESSAGE._serialized_end=548\n _POSEPOSITIONSENSORMESSAGE._serialized_start=550\n _POSEPOSITIONSENSORMESSAGE._serialized_end=646\n _CARTESIANVELOCITYSENSORMESSAGE._serialized_start=648\n _CARTESIANVELOCITYSENSORMESSAGE._serialized_end=741\n _SHOULDTERMINATESENSORMESSAGE._serialized_start=743\n _SHOULDTERMINATESENSORMESSAGE._serialized_end=818\n _CARTESIANIMPEDANCESENSORMESSAGE._serialized_start=821\n _CARTESIANIMPEDANCESENSORMESSAGE._serialized_end=952\n _FORCEPOSITIONSENSORMESSAGE._serialized_start=954\n _FORCEPOSITIONSENSORMESSAGE._serialized_end=1064\n _FORCEPOSITIONCONTROLLERSENSORMESSAGE._serialized_start=1067\n _FORCEPOSITIONCONTROLLERSENSORMESSAGE._serialized_end=1259\n _JOINTTORQUECONTROLLERSENSORMESSAGE._serialized_start=1262\n _JOINTTORQUECONTROLLERSENSORMESSAGE._serialized_end=1429\n# @@protoc_insertion_point(module_scope)\n" }, { "path": "frankapy/proto/termination_handler_params_msg.proto", "content": "syntax = \"proto2\";\n\nmessage ContactTerminationHandlerMessage {\n required double buffer_time = 1;\n\n repeated double force_thresholds = 2;\n repeated double torque_thresholds = 3;\n}\n\nmessage JointThresholdMessage {\n required double buffer_time = 1;\n\n repeated double joint_thresholds = 2;\n}\n\nmessage PoseThresholdMessage {\n required double buffer_time = 1;\n\n repeated double position_thresholds = 2;\n repeated double orientation_thresholds = 3;\n}\n\nmessage TimeTerminationHandlerMessage {\n required double buffer_time = 1;\n}" }, { "path": "frankapy/proto/termination_handler_params_msg_pb2.py", "content": "# -*- coding: utf-8 -*-\n# Generated by the protocol buffer compiler. DO NOT EDIT!\n# source: termination_handler_params_msg.proto\n\"\"\"Generated protocol buffer code.\"\"\"\nfrom google.protobuf.internal import builder as _builder\nfrom google.protobuf import descriptor as _descriptor\nfrom google.protobuf import descriptor_pool as _descriptor_pool\nfrom google.protobuf import symbol_database as _symbol_database\n# @@protoc_insertion_point(imports)\n\n_sym_db = _symbol_database.Default()\n\n\n\n\nDESCRIPTOR = _descriptor_pool.Default().AddSerializedFile(b'\\n$termination_handler_params_msg.proto\\\"l\\n ContactTerminationHandlerMessage\\x12\\x13\\n\\x0b\\x62uffer_time\\x18\\x01 \\x02(\\x01\\x12\\x18\\n\\x10\\x66orce_thresholds\\x18\\x02 \\x03(\\x01\\x12\\x19\\n\\x11torque_thresholds\\x18\\x03 \\x03(\\x01\\\"F\\n\\x15JointThresholdMessage\\x12\\x13\\n\\x0b\\x62uffer_time\\x18\\x01 \\x02(\\x01\\x12\\x18\\n\\x10joint_thresholds\\x18\\x02 \\x03(\\x01\\\"h\\n\\x14PoseThresholdMessage\\x12\\x13\\n\\x0b\\x62uffer_time\\x18\\x01 \\x02(\\x01\\x12\\x1b\\n\\x13position_thresholds\\x18\\x02 \\x03(\\x01\\x12\\x1e\\n\\x16orientation_thresholds\\x18\\x03 \\x03(\\x01\\\"4\\n\\x1dTimeTerminationHandlerMessage\\x12\\x13\\n\\x0b\\x62uffer_time\\x18\\x01 \\x02(\\x01')\n\n_builder.BuildMessageAndEnumDescriptors(DESCRIPTOR, globals())\n_builder.BuildTopDescriptorsAndMessages(DESCRIPTOR, 'termination_handler_params_msg_pb2', globals())\nif _descriptor._USE_C_DESCRIPTORS == False:\n\n DESCRIPTOR._options = None\n _CONTACTTERMINATIONHANDLERMESSAGE._serialized_start=40\n _CONTACTTERMINATIONHANDLERMESSAGE._serialized_end=148\n _JOINTTHRESHOLDMESSAGE._serialized_start=150\n _JOINTTHRESHOLDMESSAGE._serialized_end=220\n _POSETHRESHOLDMESSAGE._serialized_start=222\n _POSETHRESHOLDMESSAGE._serialized_end=326\n _TIMETERMINATIONHANDLERMESSAGE._serialized_start=328\n _TIMETERMINATIONHANDLERMESSAGE._serialized_end=380\n# @@protoc_insertion_point(module_scope)\n" }, { "path": "frankapy/proto/trajectory_generator_params_msg.proto", "content": "syntax = \"proto2\";\n\nmessage GripperTrajectoryGeneratorMessage {\n required bool grasp = 1;\n required double width = 2;\n required double speed = 3;\n required double force = 4;\n}\n\nmessage ImpulseTrajectoryGeneratorMessage {\n required double run_time = 1;\n required double acc_time = 2;\n required double max_trans = 3;\n required double max_rot = 4;\n\n repeated double forces = 5;\n repeated double torques = 6;\n}\n\nmessage JointTrajectoryGeneratorMessage {\n required double run_time = 1;\n\n repeated double joints = 2;\n}\n\nmessage JointVelocityTrajectoryGeneratorMessage {\n required double run_time = 1;\n\n repeated double joint_velocities = 2;\n repeated double joint_accelerations = 3;\n}\n\nmessage CartesianVelocityTrajectoryGeneratorMessage {\n required double run_time = 1;\n\n repeated double cartesian_velocities = 2;\n repeated double cartesian_accelerations = 3;\n}\n\nmessage PoseTrajectoryGeneratorMessage {\n required double run_time = 1;\n\n repeated double position = 2;\n repeated double quaternion = 3;\n repeated double pose = 4;\n}\n\nmessage JointDMPTrajectoryGeneratorMessage {\n required double run_time = 1;\n required double tau = 2;\n required double alpha = 3;\n required double beta = 4;\n required double num_basis = 5;\n required double num_sensor_values = 6;\n\n repeated double basis_mean = 7;\n repeated double basis_std = 8;\n repeated double weights = 9;\n repeated double initial_sensor_values = 10;\n}\n\nmessage PoseDMPTrajectoryGeneratorMessage {\n required bool orientation_only = 1;\n required bool position_only = 2;\n required bool ee_frame = 3;\n required double run_time = 4;\n required double tau = 5;\n required double alpha = 6;\n required double beta = 7;\n required double num_basis = 8;\n required double num_sensor_values = 9;\n \n repeated double basis_mean = 10;\n repeated double basis_std = 11;\n repeated double weights = 12;\n repeated double initial_sensor_values = 13;\n}\n\nmessage QuaternionPoseDMPTrajectoryGeneratorMessage {\n required bool ee_frame = 1;\n required double run_time = 2;\n required double tau_pos = 3;\n required double alpha_pos = 4;\n required double beta_pos = 5;\n required double tau_quat = 6;\n required double alpha_quat = 7;\n required double beta_quat = 8;\n\n required double num_basis_pos = 9;\n required double num_sensor_values_pos = 10;\n required double num_basis_quat = 11;\n required double num_sensor_values_quat = 12;\n\n repeated double pos_basis_mean = 13;\n repeated double pos_basis_std = 14;\n repeated double pos_weights = 15;\n repeated double pos_initial_sensor_values = 16;\n\n repeated double quat_basis_mean = 17;\n repeated double quat_basis_std = 18;\n repeated double quat_weights = 19;\n repeated double quat_initial_sensor_values = 20;\n\n required double goal_time_quat = 21;\n required double goal_quat_w = 22;\n required double goal_quat_x = 23;\n required double goal_quat_y = 24;\n required double goal_quat_z = 25;\n}\n\nmessage RunTimeMessage {\n required double run_time = 1;\n}" }, { "path": "frankapy/proto/trajectory_generator_params_msg_pb2.py", "content": "# -*- coding: utf-8 -*-\n# Generated by the protocol buffer compiler. DO NOT EDIT!\n# source: trajectory_generator_params_msg.proto\n\"\"\"Generated protocol buffer code.\"\"\"\nfrom google.protobuf.internal import builder as _builder\nfrom google.protobuf import descriptor as _descriptor\nfrom google.protobuf import descriptor_pool as _descriptor_pool\nfrom google.protobuf import symbol_database as _symbol_database\n# @@protoc_insertion_point(imports)\n\n_sym_db = _symbol_database.Default()\n\n\n\n\nDESCRIPTOR = _descriptor_pool.Default().AddSerializedFile(b'\\n%trajectory_generator_params_msg.proto\\\"_\\n!GripperTrajectoryGeneratorMessage\\x12\\r\\n\\x05grasp\\x18\\x01 \\x02(\\x08\\x12\\r\\n\\x05width\\x18\\x02 \\x02(\\x01\\x12\\r\\n\\x05speed\\x18\\x03 \\x02(\\x01\\x12\\r\\n\\x05\\x66orce\\x18\\x04 \\x02(\\x01\\\"\\x8c\\x01\\n!ImpulseTrajectoryGeneratorMessage\\x12\\x10\\n\\x08run_time\\x18\\x01 \\x02(\\x01\\x12\\x10\\n\\x08\\x61\\x63\\x63_time\\x18\\x02 \\x02(\\x01\\x12\\x11\\n\\tmax_trans\\x18\\x03 \\x02(\\x01\\x12\\x0f\\n\\x07max_rot\\x18\\x04 \\x02(\\x01\\x12\\x0e\\n\\x06\\x66orces\\x18\\x05 \\x03(\\x01\\x12\\x0f\\n\\x07torques\\x18\\x06 \\x03(\\x01\\\"C\\n\\x1fJointTrajectoryGeneratorMessage\\x12\\x10\\n\\x08run_time\\x18\\x01 \\x02(\\x01\\x12\\x0e\\n\\x06joints\\x18\\x02 \\x03(\\x01\\\"r\\n\\'JointVelocityTrajectoryGeneratorMessage\\x12\\x10\\n\\x08run_time\\x18\\x01 \\x02(\\x01\\x12\\x18\\n\\x10joint_velocities\\x18\\x02 \\x03(\\x01\\x12\\x1b\\n\\x13joint_accelerations\\x18\\x03 \\x03(\\x01\\\"~\\n+CartesianVelocityTrajectoryGeneratorMessage\\x12\\x10\\n\\x08run_time\\x18\\x01 \\x02(\\x01\\x12\\x1c\\n\\x14\\x63\\x61rtesian_velocities\\x18\\x02 \\x03(\\x01\\x12\\x1f\\n\\x17\\x63\\x61rtesian_accelerations\\x18\\x03 \\x03(\\x01\\\"f\\n\\x1ePoseTrajectoryGeneratorMessage\\x12\\x10\\n\\x08run_time\\x18\\x01 \\x02(\\x01\\x12\\x10\\n\\x08position\\x18\\x02 \\x03(\\x01\\x12\\x12\\n\\nquaternion\\x18\\x03 \\x03(\\x01\\x12\\x0c\\n\\x04pose\\x18\\x04 \\x03(\\x01\\\"\\xe5\\x01\\n\\\"JointDMPTrajectoryGeneratorMessage\\x12\\x10\\n\\x08run_time\\x18\\x01 \\x02(\\x01\\x12\\x0b\\n\\x03tau\\x18\\x02 \\x02(\\x01\\x12\\r\\n\\x05\\x61lpha\\x18\\x03 \\x02(\\x01\\x12\\x0c\\n\\x04\\x62\\x65ta\\x18\\x04 \\x02(\\x01\\x12\\x11\\n\\tnum_basis\\x18\\x05 \\x02(\\x01\\x12\\x19\\n\\x11num_sensor_values\\x18\\x06 \\x02(\\x01\\x12\\x12\\n\\nbasis_mean\\x18\\x07 \\x03(\\x01\\x12\\x11\\n\\tbasis_std\\x18\\x08 \\x03(\\x01\\x12\\x0f\\n\\x07weights\\x18\\t \\x03(\\x01\\x12\\x1d\\n\\x15initial_sensor_values\\x18\\n \\x03(\\x01\\\"\\xa7\\x02\\n!PoseDMPTrajectoryGeneratorMessage\\x12\\x18\\n\\x10orientation_only\\x18\\x01 \\x02(\\x08\\x12\\x15\\n\\rposition_only\\x18\\x02 \\x02(\\x08\\x12\\x10\\n\\x08\\x65\\x65_frame\\x18\\x03 \\x02(\\x08\\x12\\x10\\n\\x08run_time\\x18\\x04 \\x02(\\x01\\x12\\x0b\\n\\x03tau\\x18\\x05 \\x02(\\x01\\x12\\r\\n\\x05\\x61lpha\\x18\\x06 \\x02(\\x01\\x12\\x0c\\n\\x04\\x62\\x65ta\\x18\\x07 \\x02(\\x01\\x12\\x11\\n\\tnum_basis\\x18\\x08 \\x02(\\x01\\x12\\x19\\n\\x11num_sensor_values\\x18\\t \\x02(\\x01\\x12\\x12\\n\\nbasis_mean\\x18\\n \\x03(\\x01\\x12\\x11\\n\\tbasis_std\\x18\\x0b \\x03(\\x01\\x12\\x0f\\n\\x07weights\\x18\\x0c \\x03(\\x01\\x12\\x1d\\n\\x15initial_sensor_values\\x18\\r \\x03(\\x01\\\"\\xec\\x04\\n+QuaternionPoseDMPTrajectoryGeneratorMessage\\x12\\x10\\n\\x08\\x65\\x65_frame\\x18\\x01 \\x02(\\x08\\x12\\x10\\n\\x08run_time\\x18\\x02 \\x02(\\x01\\x12\\x0f\\n\\x07tau_pos\\x18\\x03 \\x02(\\x01\\x12\\x11\\n\\talpha_pos\\x18\\x04 \\x02(\\x01\\x12\\x10\\n\\x08\\x62\\x65ta_pos\\x18\\x05 \\x02(\\x01\\x12\\x10\\n\\x08tau_quat\\x18\\x06 \\x02(\\x01\\x12\\x12\\n\\nalpha_quat\\x18\\x07 \\x02(\\x01\\x12\\x11\\n\\tbeta_quat\\x18\\x08 \\x02(\\x01\\x12\\x15\\n\\rnum_basis_pos\\x18\\t \\x02(\\x01\\x12\\x1d\\n\\x15num_sensor_values_pos\\x18\\n \\x02(\\x01\\x12\\x16\\n\\x0enum_basis_quat\\x18\\x0b \\x02(\\x01\\x12\\x1e\\n\\x16num_sensor_values_quat\\x18\\x0c \\x02(\\x01\\x12\\x16\\n\\x0epos_basis_mean\\x18\\r \\x03(\\x01\\x12\\x15\\n\\rpos_basis_std\\x18\\x0e \\x03(\\x01\\x12\\x13\\n\\x0bpos_weights\\x18\\x0f \\x03(\\x01\\x12!\\n\\x19pos_initial_sensor_values\\x18\\x10 \\x03(\\x01\\x12\\x17\\n\\x0fquat_basis_mean\\x18\\x11 \\x03(\\x01\\x12\\x16\\n\\x0equat_basis_std\\x18\\x12 \\x03(\\x01\\x12\\x14\\n\\x0cquat_weights\\x18\\x13 \\x03(\\x01\\x12\\\"\\n\\x1aquat_initial_sensor_values\\x18\\x14 \\x03(\\x01\\x12\\x16\\n\\x0egoal_time_quat\\x18\\x15 \\x02(\\x01\\x12\\x13\\n\\x0bgoal_quat_w\\x18\\x16 \\x02(\\x01\\x12\\x13\\n\\x0bgoal_quat_x\\x18\\x17 \\x02(\\x01\\x12\\x13\\n\\x0bgoal_quat_y\\x18\\x18 \\x02(\\x01\\x12\\x13\\n\\x0bgoal_quat_z\\x18\\x19 \\x02(\\x01\\\"\\\"\\n\\x0eRunTimeMessage\\x12\\x10\\n\\x08run_time\\x18\\x01 \\x02(\\x01')\n\n_builder.BuildMessageAndEnumDescriptors(DESCRIPTOR, globals())\n_builder.BuildTopDescriptorsAndMessages(DESCRIPTOR, 'trajectory_generator_params_msg_pb2', globals())\nif _descriptor._USE_C_DESCRIPTORS == False:\n\n DESCRIPTOR._options = None\n _GRIPPERTRAJECTORYGENERATORMESSAGE._serialized_start=41\n _GRIPPERTRAJECTORYGENERATORMESSAGE._serialized_end=136\n _IMPULSETRAJECTORYGENERATORMESSAGE._serialized_start=139\n _IMPULSETRAJECTORYGENERATORMESSAGE._serialized_end=279\n _JOINTTRAJECTORYGENERATORMESSAGE._serialized_start=281\n _JOINTTRAJECTORYGENERATORMESSAGE._serialized_end=348\n _JOINTVELOCITYTRAJECTORYGENERATORMESSAGE._serialized_start=350\n _JOINTVELOCITYTRAJECTORYGENERATORMESSAGE._serialized_end=464\n _CARTESIANVELOCITYTRAJECTORYGENERATORMESSAGE._serialized_start=466\n _CARTESIANVELOCITYTRAJECTORYGENERATORMESSAGE._serialized_end=592\n _POSETRAJECTORYGENERATORMESSAGE._serialized_start=594\n _POSETRAJECTORYGENERATORMESSAGE._serialized_end=696\n _JOINTDMPTRAJECTORYGENERATORMESSAGE._serialized_start=699\n _JOINTDMPTRAJECTORYGENERATORMESSAGE._serialized_end=928\n _POSEDMPTRAJECTORYGENERATORMESSAGE._serialized_start=931\n _POSEDMPTRAJECTORYGENERATORMESSAGE._serialized_end=1226\n _QUATERNIONPOSEDMPTRAJECTORYGENERATORMESSAGE._serialized_start=1229\n _QUATERNIONPOSEDMPTRAJECTORYGENERATORMESSAGE._serialized_end=1849\n _RUNTIMEMESSAGE._serialized_start=1851\n _RUNTIMEMESSAGE._serialized_end=1885\n# @@protoc_insertion_point(module_scope)\n" }, { "path": "frankapy/proto_utils.py", "content": "from franka_interface_msgs.msg import SensorData, SensorDataGroup\n\n\ndef sensor_proto2ros_msg(sensor_proto_msg, sensor_data_type, info=''):\n sensor_ros_msg = SensorData()\n sensor_ros_msg.type = sensor_data_type\n sensor_ros_msg.info = info \n\n sensor_data_bytes = sensor_proto_msg.SerializeToString()\n sensor_ros_msg.size = len(sensor_data_bytes)\n sensor_ros_msg.sensorData = sensor_data_bytes\n\n return sensor_ros_msg\n\n\ndef make_sensor_group_msg(trajectory_generator_sensor_msg=None, feedback_controller_sensor_msg=None, termination_handler_sensor_msg=None):\n sensor_group_msg = SensorDataGroup()\n\n if trajectory_generator_sensor_msg is not None:\n sensor_group_msg.has_trajectory_generator_sensor_data = True\n sensor_group_msg.trajectoryGeneratorSensorData = trajectory_generator_sensor_msg\n if feedback_controller_sensor_msg is not None:\n sensor_group_msg.has_feedback_controller_sensor_data = True\n sensor_group_msg.feedbackControllerSensorData = feedback_controller_sensor_msg\n if termination_handler_sensor_msg is not None:\n sensor_group_msg.has_termination_handler_sensor_data = True\n sensor_group_msg.terminationHandlerSensorData = termination_handler_sensor_msg\n\n return sensor_group_msg\n" }, { "path": "frankapy/ros_utils.py", "content": "import rospy\nfrom visualization_msgs.msg import Marker, MarkerArray\nimport quaternion\n\n\nclass BoxesPublisher:\n\n def __init__(self, name, world_frame='panda_link0'):\n self._boxes_pub = rospy.Publisher(name, MarkerArray, queue_size=10)\n self._world_frame = world_frame\n\n def publish_boxes(self, boxes):\n markers = []\n for i, box in enumerate(boxes):\n marker = Marker()\n marker.type = Marker.CUBE\n marker.header.stamp = rospy.Time.now()\n marker.header.frame_id = self._world_frame\n marker.id = i\n\n marker.lifetime = rospy.Duration()\n\n marker.pose.position.x = box[0]\n marker.pose.position.y = box[1]\n marker.pose.position.z = box[2]\n\n marker.scale.x = box[-3]\n marker.scale.y = box[-2]\n marker.scale.z = box[-1]\n\n if len(box) == 9:\n q = quaternion.from_euler_angles(box[3], box[4], box[5])\n for k in 'wxyz':\n setattr(marker.pose.orientation, k, getattr(q, k))\n elif len(box) == 10:\n for j, k in enumerate('wxyz'):\n setattr(marker.pose.orientation, k, box[3 + j])\n else:\n raise ValueError('Invalid format for box!')\n\n marker.color.r = 0.5\n marker.color.g = 0.5\n marker.color.b = 0.5\n marker.color.a = 0.6\n\n markers.append(marker)\n\n marker_array = MarkerArray(markers)\n self._boxes_pub.publish(marker_array)" }, { "path": "frankapy/skill_list.py", "content": "import roslib\nroslib.load_manifest('franka_interface_msgs')\nimport rospy\nimport actionlib\nimport numpy as np\nfrom autolab_core import RigidTransform\n\nfrom .franka_constants import FrankaConstants as FC\nfrom .franka_interface_common_definitions import *\nfrom .proto import *\nfrom .utils import transform_to_list\n\nfrom franka_interface_msgs.msg import ExecuteSkillAction, ExecuteSkillGoal\n\nclass Skill:\n def __init__(self, \n skill_type,\n trajectory_generator_type,\n feedback_controller_type = FeedbackControllerType.NoopFeedbackController,\n termination_handler_type = TerminationHandlerType.NoopTerminationHandler,\n meta_skill_type = MetaSkillType.BaseMetaSkill,\n meta_skill_id = 0,\n sensor_topics = None,\n timer_type = 1,\n skill_desc = ''):\n self._skill_type = skill_type\n self._skill_desc = skill_desc\n self._meta_skill_type = meta_skill_type\n self._meta_skill_id = meta_skill_id\n self._sensor_topics = sensor_topics if sensor_topics is not None else ['']\n self._trajectory_generator_type = trajectory_generator_type\n self._feedback_controller_type = feedback_controller_type\n self._termination_handler_type = termination_handler_type\n self._timer_type = timer_type\n\n self._sensor_value_sizes = 0\n self._initial_sensor_values = []\n\n # Add trajectory params\n self._trajectory_generator_param_data = []\n self._trajectory_generator_param_data_size = 0\n\n # Add feedback controller params\n self._feedback_controller_param_data = []\n self._feedback_controller_param_data_size = 0\n\n # Add termination params\n self._termination_handler_param_data = []\n self._termination_handler_param_data_size = 0\n\n # Add timer params\n self._timer_params = []\n self._num_timer_params = 0\n\n def set_meta_skill_type(self, meta_skill_type):\n self._meta_skill_type = meta_skill_type\n\n def set_meta_skill_id(self, meta_skill_id):\n self._meta_skill_id = meta_skill_id\n\n def add_initial_sensor_values(self, values):\n self._initial_sensor_values = values\n self._sensor_value_sizes = [len(values)]\n\n def add_trajectory_params(self, params):\n self._trajectory_generator_param_data = params\n self._trajectory_generator_param_data_size = len(params)\n\n def add_feedback_controller_params(self, params):\n self._feedback_controller_param_data = params\n self._feedback_controller_param_data_size = len(params)\n\n def add_termination_params(self, params):\n self._termination_handler_param_data = params\n self._termination_handler_param_data_size = len(params)\n\n def add_timer_params(self, params):\n self._timer_params = params\n self._num_timer_params = len(params)\n\n ## Feedback Controllers\n\n def set_cartesian_impedances(self, use_impedance, cartesian_impedances, joint_impedances):\n if use_impedance:\n if cartesian_impedances is not None:\n self.add_cartesian_impedances(cartesian_impedances)\n else:\n self.add_cartesian_impedances(FC.DEFAULT_TRANSLATIONAL_STIFFNESSES + FC.DEFAULT_ROTATIONAL_STIFFNESSES)\n else:\n if joint_impedances is not None:\n self.add_internal_impedances([], joint_impedances)\n elif cartesian_impedances is not None:\n self.add_internal_impedances(cartesian_impedances, [])\n else:\n self.add_internal_impedances([], FC.DEFAULT_JOINT_IMPEDANCES)\n\n def set_joint_impedances(self, use_impedance, cartesian_impedances, joint_impedances, k_gains, d_gains):\n if use_impedance:\n if k_gains is not None and d_gains is not None:\n self.add_joint_gains(k_gains, d_gains)\n else:\n self.add_joint_gains(FC.DEFAULT_K_GAINS, FC.DEFAULT_D_GAINS)\n else:\n if joint_impedances is not None:\n self.add_internal_impedances([], joint_impedances)\n elif cartesian_impedances is not None:\n self.add_internal_impedances(cartesian_impedances, [])\n else:\n self.add_internal_impedances([], FC.DEFAULT_JOINT_IMPEDANCES)\n\n def add_cartesian_impedances(self, cartesian_impedances):\n assert type(cartesian_impedances) is list, \\\n \"Incorrect cartesian impedances type. Should be list.\"\n assert len(cartesian_impedances) == 6, \\\n \"Incorrect cartesian impedances len. Should be 6.\"\n assert self._skill_type == SkillType.ImpedanceControlSkill, \\\n \"Incorrect skill type. Should be ImpedanceControlSkill.\"\n\n cartesian_impedance_feedback_controller_msg_proto = \\\n CartesianImpedanceFeedbackControllerMessage(\n translational_stiffnesses=cartesian_impedances[:3], \n rotational_stiffnesses=cartesian_impedances[3:])\n\n self.add_feedback_controller_params(cartesian_impedance_feedback_controller_msg_proto.SerializeToString())\n\n def add_force_axis_params(self, translational_stiffness, rotational_stiffness, axis):\n assert type(translational_stiffness) is float or type(translational_stiffness) is int, \\\n \"Incorrect translational stiffness type. Should be int or float.\"\n assert type(rotational_stiffness) is float or type(rotational_stiffness) is int, \\\n \"Incorrect rotational stiffness type. Should be int or float.\"\n assert type(axis) is list, \\\n \"Incorrect axis type. Should be list.\"\n assert len(axis) == 3, \\\n \"Incorrect axis len. Should be 3.\"\n\n force_axis_feedback_controller_msg_proto = \\\n ForceAxisFeedbackControllerMessage(translational_stiffness=translational_stiffness,\n rotational_stiffness=rotational_stiffness, \n axis=axis)\n\n self.add_feedback_controller_params(force_axis_feedback_controller_msg_proto.SerializeToString())\n\n def add_internal_impedances(self, cartesian_impedances, joint_impedances):\n assert type(cartesian_impedances) is list, \\\n \"Incorrect joint impedances type. Should be list.\"\n assert type(joint_impedances) is list, \\\n \"Incorrect joint impedances type. Should be list.\"\n assert len(cartesian_impedances) == 0 or len(cartesian_impedances) == 6, \\\n \"Incorrect cartesian impedances len. Should be 0 or 6.\"\n assert len(joint_impedances) == 0 or len(joint_impedances) == 7, \\\n \"Incorrect joint impedances len. Should be 0 or 7.\"\n assert self._skill_type == SkillType.CartesianPoseSkill or self._skill_type == SkillType.JointPositionSkill \\\n or self._skill_type == SkillType.CartesianVelocitySkill or self._skill_type == SkillType.JointVelocitySkill, \\\n \"Incorrect skill type. Should be CartesianPoseSkill, JointPositionSkill, CartesianVelocitySkill, or JointVelocitySkill.\"\n internal_feedback_controller_msg_proto = \\\n InternalImpedanceFeedbackControllerMessage(\n cartesian_impedances=cartesian_impedances, joint_impedances=joint_impedances)\n\n self.add_feedback_controller_params(internal_feedback_controller_msg_proto.SerializeToString())\n\n def add_joint_torques(self, joint_torques, selection, remove_gravity, k_gains, d_gains):\n assert type(joint_torques) is list, \"Incorrect joint_torques type. Should be list.\"\n assert type(selection) is list, \"Incorrect selection type. Should be list.\"\n assert type(remove_gravity) is list, \"Incorrect remove_gravity type. Should be list.\"\n assert type(k_gains) is list, \"Incorrect k_gains type. Should be list.\"\n assert type(d_gains) is list, \"Incorrect d_gains type. Should be list.\"\n assert len(joint_torques) == 7, \"Incorrect joint_torques len. Should be 7.\"\n assert len(selection) == 7, \"Incorrect selection len. Should be 7.\"\n assert len(remove_gravity) == 7, \"Incorrect remove_gravity len. Should be 7.\"\n assert len(k_gains) == 7, \"Incorrect k_gains len. Should be 7.\"\n assert len(d_gains) == 7, \"Incorrect d_gains len. Should be 7.\"\n assert self._skill_type == SkillType.ImpedanceControlSkill, \\\n \"Incorrect skill type. Should be ImpedanceControlSkill\"\n\n joint_torque_controller_msg_proto = \\\n JointTorqueFeedbackControllerMessage(joint_torques=joint_torques, selection=selection, remove_gravity=remove_gravity, k_gains=k_gains, d_gains=d_gains)\n\n self.add_feedback_controller_params(joint_torque_controller_msg_proto.SerializeToString())\n\n def add_force_position_params(self, position_kps_cart, force_kps_cart, position_kps_joint, force_kps_joint, S, use_cartesian_gains):\n assert type(position_kps_cart) is list or len(position_kps_cart) == 6, \\\n \"Incorrect position_kps_cart type. Should be list of length 6.\"\n assert type(force_kps_cart) is list or len(force_kps_cart) == 6, \\\n \"Incorrect force_kps_cart type. Should be list of length 6.\"\n assert type(position_kps_joint) is list or len(position_kps_joint) == 7, \\\n \"Incorrect position_kps_joint type. Should be list of length 7.\"\n assert type(force_kps_joint) is list or len(force_kps_joint) == 7, \\\n \"Incorrect force_kps_joint type. Should be list of length 7.\"\n assert type(S) is list and len(S) == 6, \\\n \"Incorrect S type. Should be list of length 6.\"\n assert type(use_cartesian_gains) is bool, \\\n \"Incorrect use_cartesian_gains type. Should be bool.\"\n\n force_position_feedback_controller_msg_proto = \\\n ForcePositionFeedbackControllerMessage(\n position_kps_cart=position_kps_cart, force_kps_cart=force_kps_cart, \n position_kps_joint=position_kps_joint, force_kps_joint=force_kps_joint,\n selection=S, use_cartesian_gains=use_cartesian_gains)\n \n self.add_feedback_controller_params(force_position_feedback_controller_msg_proto.SerializeToString())\n\n def add_joint_gains(self, k_gains, d_gains):\n assert type(k_gains) is list, \"Incorrect k_gains type. Should be list.\"\n assert type(d_gains) is list, \"Incorrect d_gains type. Should be list.\"\n assert len(k_gains) == 7, \"Incorrect k_gains len. Should be 7.\"\n assert len(d_gains) == 7, \"Incorrect d_gains len. Should be 7.\"\n assert self._skill_type == SkillType.ImpedanceControlSkill, \\\n \"Incorrect skill type. Should be ImpedanceControlSkill\"\n\n joint_feedback_controller_msg_proto = \\\n JointImpedanceFeedbackControllerMessage(k_gains=k_gains, d_gains=d_gains)\n\n self.add_feedback_controller_params(joint_feedback_controller_msg_proto.SerializeToString())\n \n ## Termination Handlers\n\n def check_for_contact_params(self, buffer_time, force_thresholds, torque_thresholds):\n if force_thresholds is not None or torque_thresholds is not None:\n self._termination_handler_type = TerminationHandlerType.ContactTerminationHandler\n\n if force_thresholds is None:\n force_thresholds = []\n if torque_thresholds is None:\n torque_thresholds = []\n self.add_contact_termination_params(buffer_time,\n force_thresholds,\n torque_thresholds)\n return True\n else:\n return False\n\n def add_contact_termination_params(self, buffer_time,\n force_thresholds,\n torque_thresholds):\n assert type(buffer_time) is float or type(buffer_time) is int, \\\n \"Incorrect buffer time type. Should be int or float.\"\n assert buffer_time >= 0, \"Incorrect buffer time. Should be non negative.\"\n assert type(force_thresholds) is list, \\\n \"Incorrect force thresholds type. Should be list.\"\n assert type(torque_thresholds) is list, \\\n \"Incorrect torque thresholds type. Should be list.\"\n assert len(force_thresholds) == 0 or len(force_thresholds) == 6, \\\n \"Incorrect force thresholds length. Should be 0 or 6.\"\n assert len(torque_thresholds) == 0 or len(torque_thresholds) == 7, \\\n \"Incorrect torque thresholds length. Should be 0 or 7.\"\n assert self._termination_handler_type == TerminationHandlerType.ContactTerminationHandler, \\\n \"Incorrect termination handler type. Should be ContactTerminationHandler\"\n\n contact_termination_handler_msg_proto = \\\n ContactTerminationHandlerMessage(buffer_time=buffer_time, force_thresholds=force_thresholds,\n torque_thresholds=torque_thresholds)\n\n self.add_termination_params(contact_termination_handler_msg_proto.SerializeToString())\n\n def add_joint_threshold_params(self, buffer_time, joint_thresholds):\n assert type(buffer_time) is float or type(buffer_time) is int, \\\n \"Incorrect buffer time type. Should be int or float.\"\n assert buffer_time >= 0, \"Incorrect buffer time. Should be non negative.\"\n assert type(joint_thresholds) is list, \\\n \"Incorrect joint thresholds type. Should be list.\"\n assert len(joint_thresholds) == 0 or len(joint_thresholds) == 7, \\\n \"Incorrect joint thresholds length. Should be 0 or 7.\"\n assert self._termination_handler_type == TerminationHandlerType.FinalJointTerminationHandler, \\\n \"Incorrect termination handler type. Should be FinalJointTerminationHandler\"\n\n joint_threshold_msg_proto = JointThresholdMessage(buffer_time=buffer_time, \n joint_thresholds=joint_thresholds)\n\n self.add_termination_params(joint_threshold_msg_proto.SerializeToString())\n\n def add_pose_threshold_params(self, buffer_time, pose_thresholds):\n assert type(buffer_time) is float or type(buffer_time) is int, \\\n \"Incorrect buffer time type. Should be int or float.\"\n assert buffer_time >= 0, \"Incorrect buffer time. Should be non negative.\"\n assert type(pose_thresholds) is list, \\\n \"Incorrect pose thresholds type. Should be list.\"\n assert len(pose_thresholds) == 0 or len(pose_thresholds) == 6, \\\n \"Incorrect pose thresholds length. Should be 0 or 6.\"\n assert self._termination_handler_type == TerminationHandlerType.FinalPoseTerminationHandler, \\\n \"Incorrect termination handler type. Should be FinalPoseTerminationHandler\"\n\n pose_threshold_msg_proto = PoseThresholdMessage(buffer_time=buffer_time, \n position_thresholds=pose_thresholds[:3], \n orientation_thresholds=pose_thresholds[3:])\n\n self.add_termination_params(pose_threshold_msg_proto.SerializeToString())\n\n def add_time_termination_params(self, buffer_time):\n assert type(buffer_time) is float or type(buffer_time) is int, \\\n \"Incorrect buffer time type. Should be int or float.\"\n assert buffer_time >= 0, \"Incorrect buffer time. Should be non negative.\"\n assert self._termination_handler_type == TerminationHandlerType.TimeTerminationHandler, \\\n \"Incorrect termination handler type. Should be TimeTerminationHandler\"\n\n time_termination_handler_msg_proto = TimeTerminationHandlerMessage(buffer_time=buffer_time)\n\n self.add_termination_params(time_termination_handler_msg_proto.SerializeToString())\n\n ## Trajectory Generators\n\n def add_gripper_params(self, grasp, width, speed, force):\n assert type(grasp) is bool, \\\n \"Incorrect grasp type. Should be bool.\"\n assert type(width) is float or type(width) is int, \\\n \"Incorrect width type. Should be int or float.\"\n assert type(speed) is float or type(speed) is int, \\\n \"Incorrect speed type. Should be int or float.\"\n assert type(force) is float or type(force) is int, \\\n \"Incorrect force type. Should be int or float.\"\n assert width >= 0, \"Incorrect width. Should be non negative.\"\n assert speed >= 0, \"Incorrect speed. Should be non negative.\"\n assert force >= 0, \"Incorrect force. Should be non negative.\"\n assert self._skill_type == SkillType.GripperSkill, \\\n \"Incorrect skill type. Should be GripperSkill\"\n assert self._trajectory_generator_type == TrajectoryGeneratorType.GripperTrajectoryGenerator, \\\n \"Incorrect trajectory generator type. Should be GripperTrajectoryGenerator\"\n\n gripper_trajectory_generator_msg_proto = GripperTrajectoryGeneratorMessage(\n grasp=grasp, width=width, speed=speed, force=force)\n\n self.add_trajectory_params(gripper_trajectory_generator_msg_proto.SerializeToString())\n\n def add_impulse_params(self, run_time, acc_time, max_trans, max_rot, forces, torques):\n assert type(run_time) is float or type(run_time) is int, \\\n \"Incorrect run_time type. Should be int or float.\"\n assert run_time >= 0, \"Incorrect run_time. Should be non negative.\"\n assert type(acc_time) is float or type(acc_time) is int, \\\n \"Incorrect acc time type. Should be int or float.\"\n assert acc_time >= 0, \"Incorrect acc time. Should be non negative.\"\n assert type(max_trans) is float or type(max_trans) is int, \\\n \"Incorrect max trans type. Should be int or float.\"\n assert max_trans >= 0, \"Incorrect max trans. Should be non negative.\"\n assert type(max_rot) is float or type(max_rot) is int, \\\n \"Incorrect max rot type. Should be int or float.\"\n assert max_rot >= 0, \"Incorrect max rot. Should be non negative.\"\n assert type(forces) is list, \"Incorrect forces type. Should be list.\"\n assert len(forces) == 3, \"Incorrect forces len. Should be 3.\"\n assert type(torques) is list, \"Incorrect torques type. Should be list.\"\n assert len(torques) == 3, \"Incorrect torques len. Should be 3.\"\n\n impulse_trajectory_generator_msg_proto = ImpulseTrajectoryGeneratorMessage(\n run_time=run_time, acc_time=acc_time, max_trans=max_trans, \n max_rot=max_rot, forces=forces, torques=torques)\n\n self.add_trajectory_params(impulse_trajectory_generator_msg_proto.SerializeToString())\n\n def add_goal_cartesian_velocities(self, run_time, cartesian_velocities, cartesian_accelerations):\n assert type(run_time) is float or type(run_time) is int,\\\n \"Incorrect time type. Should be int or float.\"\n assert run_time >= 0, \"Incorrect time. Should be non negative.\"\n assert type(cartesian_velocities) is list, \"Incorrect cartesian_velocities type. Should be list.\"\n assert len(cartesian_velocities) == 6, \"Incorrect cartesian_velocities len. Should be 7.\"\n assert type(cartesian_accelerations) is list, \"Incorrect cartesian_accelerations type. Should be list.\"\n assert len(cartesian_accelerations) == 6, \"Incorrect cartesian_accelerations len. Should be 7.\"\n\n cartesian_velocity_trajectory_generator_msg_proto = CartesianVelocityTrajectoryGeneratorMessage(run_time=run_time, \n cartesian_velocities=cartesian_velocities, cartesian_accelerations=cartesian_accelerations)\n\n self.add_trajectory_params(cartesian_velocity_trajectory_generator_msg_proto.SerializeToString())\n\n def add_goal_pose(self, run_time, goal_pose):\n assert type(run_time) is float or type(run_time) is int, \\\n \"Incorrect run_time type. Should be int or float.\"\n assert run_time >= 0, \"Incorrect run_time. Should be non negative.\"\n assert type(goal_pose) is RigidTransform, \"Incorrect goal_pose type. Should be RigidTransform.\"\n\n pose_trajectory_generator_msg_proto = PoseTrajectoryGeneratorMessage(\n run_time=run_time, position=goal_pose.translation, quaternion=goal_pose.quaternion,\n pose=transform_to_list(goal_pose))\n\n self.add_trajectory_params(pose_trajectory_generator_msg_proto.SerializeToString())\n\n def add_goal_joints(self, run_time, joints):\n assert type(run_time) is float or type(run_time) is int,\\\n \"Incorrect time type. Should be int or float.\"\n assert run_time >= 0, \"Incorrect time. Should be non negative.\"\n assert type(joints) is list, \"Incorrect joints type. Should be list.\"\n assert len(joints) == 7, \"Incorrect joints len. Should be 7.\"\n\n joint_trajectory_generator_msg_proto = JointTrajectoryGeneratorMessage(run_time=run_time, joints=joints)\n\n self.add_trajectory_params(joint_trajectory_generator_msg_proto.SerializeToString())\n\n def add_goal_joint_velocities(self, run_time, joint_velocities, joint_accelerations):\n assert type(run_time) is float or type(run_time) is int,\\\n \"Incorrect time type. Should be int or float.\"\n assert run_time >= 0, \"Incorrect time. Should be non negative.\"\n assert type(joint_velocities) is list, \"Incorrect joint_velocities type. Should be list.\"\n assert len(joint_velocities) == 7, \"Incorrect joint_velocities len. Should be 7.\"\n assert type(joint_accelerations) is list, \"Incorrect joint_accelerations type. Should be list.\"\n assert len(joint_accelerations) == 7, \"Incorrect joint_accelerations len. Should be 7.\"\n\n joint_velocity_trajectory_generator_msg_proto = JointVelocityTrajectoryGeneratorMessage(run_time=run_time, \n joint_velocities=joint_velocities, joint_accelerations=joint_accelerations)\n\n self.add_trajectory_params(joint_velocity_trajectory_generator_msg_proto.SerializeToString())\n\n def add_joint_dmp_params(self, run_time, joint_dmp_info, initial_sensor_values):\n assert type(run_time) is float or type(run_time) is int,\\\n \"Incorrect run_time type. Should be int or float.\"\n assert run_time >= 0, \"Incorrect run_time. Should be non negative.\"\n\n assert type(joint_dmp_info['tau']) is float or type(joint_dmp_info['tau']) is int,\\\n \"Incorrect tau type. Should be int or float.\"\n assert joint_dmp_info['tau'] >= 0, \"Incorrect tau. Should be non negative.\"\n\n assert type(joint_dmp_info['alpha']) is float or type(joint_dmp_info['alpha']) is int,\\\n \"Incorrect alpha type. Should be int or float.\"\n assert joint_dmp_info['alpha'] >= 0, \"Incorrect alpha. Should be non negative.\"\n\n assert type(joint_dmp_info['beta']) is float or type(joint_dmp_info['beta']) is int,\\\n \"Incorrect beta type. Should be int or float.\"\n assert joint_dmp_info['beta'] >= 0, \"Incorrect beta. Should be non negative.\"\n\n assert type(joint_dmp_info['num_basis']) is float or type(joint_dmp_info['num_basis']) is int,\\\n \"Incorrect num basis type. Should be int or float.\"\n assert joint_dmp_info['num_basis'] >= 0, \"Incorrect num basis. Should be non negative.\"\n\n assert type(joint_dmp_info['num_sensors']) is float or type(joint_dmp_info['num_sensors']) is int,\\\n \"Incorrect num sensors type. Should be int or float.\"\n assert joint_dmp_info['num_sensors'] >= 0, \"Incorrect num sensors. Should be non negative.\"\n\n assert type(joint_dmp_info['mu']) is list, \"Incorrect basis mean type. Should be list.\"\n assert len(joint_dmp_info['mu']) == joint_dmp_info['num_basis'], \\\n \"Incorrect basis mean len. Should be equal to num basis.\"\n\n assert type(joint_dmp_info['h']) is list, \"Incorrect basis std dev type. Should be list.\"\n assert len(joint_dmp_info['h']) == joint_dmp_info['num_basis'], \\\n \"Incorrect basis std dev len. Should be equal to num basis.\"\n\n assert type(initial_sensor_values) is list, \"Incorrect initial sensor values type. Should be list.\"\n assert len(initial_sensor_values) == joint_dmp_info['num_sensors'], \\\n \"Incorrect initial sensor values len. Should be equal to num sensors.\"\n\n weights = np.array(joint_dmp_info['weights']).reshape(-1).tolist()\n num_weights = 7 * int(joint_dmp_info['num_basis']) * int(joint_dmp_info['num_sensors'])\n\n assert len(weights) == num_weights, \\\n \"Incorrect weights len. Should be equal to 7 * num basis * num sensors.\"\n\n assert self._skill_type == SkillType.ImpedanceControlSkill or \\\n self._skill_type == SkillType.JointPositionSkill, \\\n \"Incorrect skill type. Should be ImpedanceControlSkill or JointPositionSkill.\"\n assert self._trajectory_generator_type == TrajectoryGeneratorType.JointDmpTrajectoryGenerator, \\\n \"Incorrect trajectory generator type. Should be JointDmpTrajectoryGenerator\"\n\n joint_dmp_trajectory_generator_msg_proto = JointDMPTrajectoryGeneratorMessage(run_time=run_time, \n tau=joint_dmp_info['tau'], alpha=joint_dmp_info['alpha'], beta=joint_dmp_info['beta'], \n num_basis=joint_dmp_info['num_basis'], num_sensor_values=joint_dmp_info['num_sensors'], \n basis_mean=joint_dmp_info['mu'], basis_std=joint_dmp_info['h'], \n weights=np.array(joint_dmp_info['weights']).reshape(-1).tolist(), \n initial_sensor_values=initial_sensor_values)\n\n self.add_trajectory_params(joint_dmp_trajectory_generator_msg_proto.SerializeToString())\n\n\n def _check_dmp_info_parameters(self, dmp_info):\n assert type(dmp_info['tau']) in (float, int), \"Incorrect tau type. Should be int or float.\"\n assert dmp_info['tau'] >= 0, \"Incorrect tau. Should be non negative.\"\n\n assert type(dmp_info['alpha']) in (float, int), \"Incorrect alpha type. Should be int or float.\"\n assert dmp_info['alpha'] >= 0, \"Incorrect alpha. Should be non negative.\"\n\n assert type(dmp_info['beta']) in (float, int), \"Incorrect beta type. Should be int or float.\"\n assert dmp_info['beta'] >= 0, \"Incorrect beta. Should be non negative.\"\n\n assert type(dmp_info['num_basis']) in (float, int), \"Incorrect num basis type. Should be int or float.\"\n assert dmp_info['num_basis'] >= 0, \"Incorrect num basis. Should be non negative.\"\n\n assert type(dmp_info['num_sensors']) in (float, int), \"Incorrect num sensors type. Should be int or float.\"\n assert dmp_info['num_sensors'] >= 0, \"Incorrect num sensors. Should be non negative.\"\n\n assert type(dmp_info['mu']) is list, \"Incorrect basis mean type. Should be list.\"\n assert len(dmp_info['mu']) == dmp_info['num_basis'], \\\n \"Incorrect basis mean len. Should be equal to num basis.\"\n\n assert type(dmp_info['h']) is list, \"Incorrect basis std dev type. Should be list.\"\n assert len(dmp_info['h']) == dmp_info['num_basis'], \\\n \"Incorrect basis std dev len. Should be equal to num basis.\"\n\n def add_pose_dmp_params(self, orientation_only, position_only, ee_frame, run_time, pose_dmp_info, initial_sensor_values):\n assert type(run_time) is float or type(run_time) is int,\\\n \"Incorrect run_time type. Should be int or float.\"\n assert run_time >= 0, \"Incorrect run_time. Should be non negative.\"\n self._check_dmp_info_parameters(pose_dmp_info)\n assert type(initial_sensor_values) is list, \"Incorrect initial sensor values type. Should be list.\"\n\n weights = np.array(pose_dmp_info['weights']).reshape(-1).tolist()\n\n if orientation_only or position_only:\n num_weights = 3 * int(pose_dmp_info['num_basis']) * int(pose_dmp_info['num_sensors'])\n assert len(weights) == num_weights, \\\n \"Incorrect weights len. Should be equal to 3 * num basis * num sensors.\"\n assert len(initial_sensor_values) == 3 * pose_dmp_info['num_sensors'], \\\n \"Incorrect initial sensor values len. Should be equal to 3 * num sensors.\"\n else:\n num_weights = 6 * int(pose_dmp_info['num_basis']) * int(pose_dmp_info['num_sensors'])\n assert len(weights) == num_weights, \\\n \"Incorrect weights len. Should be equal to 6 * num basis * num sensors.\"\n assert len(initial_sensor_values) == 6 * pose_dmp_info['num_sensors'], \\\n \"Incorrect initial sensor values len. Should be equal to 3 * num sensors.\"\n\n assert self._skill_type == SkillType.CartesianPoseSkill or \\\n self._skill_type == SkillType.ImpedanceControlSkill, \\\n \"Incorrect skill type. Should be CartesianPoseSkill or ImpedanceControlSkill.\"\n assert self._trajectory_generator_type == TrajectoryGeneratorType.PoseDmpTrajectoryGenerator, \\\n \"Incorrect trajectory generator type. Should be PoseDmpTrajectoryGenerator\"\n\n pose_dmp_trajectory_generator_msg_proto = PoseDMPTrajectoryGeneratorMessage(orientation_only=orientation_only,\n position_only=position_only, ee_frame=ee_frame, run_time=run_time, \n tau=pose_dmp_info['tau'], alpha=pose_dmp_info['alpha'], beta=pose_dmp_info['beta'], \n num_basis=pose_dmp_info['num_basis'], num_sensor_values=pose_dmp_info['num_sensors'], \n basis_mean=pose_dmp_info['mu'], basis_std=pose_dmp_info['h'], \n weights=np.array(pose_dmp_info['weights']).reshape(-1).tolist(), \n initial_sensor_values=initial_sensor_values)\n\n self.add_trajectory_params(pose_dmp_trajectory_generator_msg_proto.SerializeToString())\n\n def add_quaternion_pose_dmp_params(self, ee_frame, run_time, position_dmp_info, quat_dmp_info, initial_sensor_values, goal_quat, goal_quat_time):\n assert type(run_time) is float or type(run_time) is int,\\\n \"Incorrect run_time type. Should be int or float.\"\n assert run_time >= 0, \"Incorrect run_time. Should be non negative.\"\n assert len(goal_quat) == 4, \"Invalid number of quaternion values for goal quaternion. Required: 4, provided: {}\".format(\n len(goal_quat))\n\n self._check_dmp_info_parameters(position_dmp_info)\n self._check_dmp_info_parameters(quat_dmp_info)\n assert type(initial_sensor_values) is list, \"Incorrect initial sensor values type. Should be list.\"\n\n pos_weights = np.array(position_dmp_info['weights']).reshape(-1).tolist()\n num_pos_weights = 3 * int(position_dmp_info['num_basis']) * int(position_dmp_info['num_sensors'])\n assert len(pos_weights) == num_pos_weights, \\\n \"Incorrect weights len. Should be equal to 3 * num basis * num sensors.\"\n\n quat_weights = np.array(quat_dmp_info['weights']).reshape(-1).tolist()\n num_quat_weights = 3 * int(quat_dmp_info['num_basis']) * int(quat_dmp_info['num_sensors'])\n assert len(quat_weights) == num_quat_weights, \\\n \"Incorrect weights len. Should be equal to 4 * num basis * num sensors.\"\n\n assert self._skill_type == SkillType.CartesianPoseSkill or \\\n self._skill_type == SkillType.ImpedanceControlSkill, \\\n \"Incorrect skill type. Should be CartesianPoseSkill or ImpedanceControlSkill.\"\n assert self._trajectory_generator_type == TrajectoryGeneratorType.QuaternionPoseDmpTrajectoryGenerator, \\\n \"Incorrect trajectory generator type. Should be QuaternionPoseDmpTrajectoryGenerator\"\n\n quat_pose_dmp_trajectory_generator_msg_proto = QuaternionPoseDMPTrajectoryGeneratorMessage(ee_frame=ee_frame, run_time=run_time, \n tau_pos=position_dmp_info['tau'], alpha_pos=position_dmp_info['alpha'], beta_pos=position_dmp_info['beta'],\n tau_quat=quat_dmp_info['tau'], alpha_quat=quat_dmp_info['alpha'], beta_quat=quat_dmp_info['beta'],\n num_basis_pos=position_dmp_info['num_basis'], num_sensor_values_pos=position_dmp_info['num_sensors'], \n num_basis_quat=quat_dmp_info['num_basis'], num_sensor_values_quat=quat_dmp_info['num_sensors'], \n pos_basis_mean=position_dmp_info['mu'], pos_basis_std=position_dmp_info['h'], \n quat_basis_mean=quat_dmp_info['mu'], quat_basis_std=quat_dmp_info['h'], \n pos_weights=np.array(position_dmp_info['weights']).reshape(-1).tolist(), \n quat_weights=np.array(quat_dmp_info['weights']).reshape(-1).tolist(), \n pos_initial_sensor_values=initial_sensor_values,\n quat_initial_sensor_values=initial_sensor_values,\n goal_time_quat=goal_quat_time,\n goal_quat_w=goal_quat[0], goal_quat_x=goal_quat[1], goal_quat_y=goal_quat[2], goal_quat_z=goal_quat[3])\n\n self.add_trajectory_params(quat_pose_dmp_trajectory_generator_msg_proto.SerializeToString())\n \n\n def add_run_time(self, run_time):\n assert type(run_time) is float or type(run_time) is int, \\\n \"Incorrect run_time type. Should be int or float.\"\n assert run_time >= 0, \"Incorrect run_time. Should be non negative.\"\n\n run_time_msg_proto = RunTimeMessage(run_time=run_time)\n self.add_trajectory_params(run_time_msg_proto.SerializeToString())\n\n # Add checks for these\n def create_goal(self):\n goal = ExecuteSkillGoal()\n goal.skill_type = self._skill_type\n goal.skill_description = self._skill_desc\n goal.meta_skill_type = self._meta_skill_type\n goal.meta_skill_id = self._meta_skill_id\n goal.sensor_topics = self._sensor_topics\n goal.initial_sensor_values = self._initial_sensor_values\n goal.sensor_value_sizes = self._sensor_value_sizes\n goal.trajectory_generator_type = self._trajectory_generator_type\n goal.trajectory_generator_param_data = self._trajectory_generator_param_data\n goal.trajectory_generator_param_data_size = self._trajectory_generator_param_data_size\n goal.feedback_controller_type = self._feedback_controller_type\n goal.feedback_controller_param_data = self._feedback_controller_param_data\n goal.feedback_controller_param_data_size = \\\n self._feedback_controller_param_data_size\n goal.termination_handler_type = self._termination_handler_type\n goal.termination_handler_param_data = self._termination_handler_param_data\n goal.termination_handler_param_data_size = self._termination_handler_param_data_size\n goal.timer_type = self._timer_type\n goal.timer_params = self._timer_params\n goal.num_timer_params = self._num_timer_params\n return goal\n\n def feedback_callback(self, feedback):\n pass" }, { "path": "frankapy/utils.py", "content": "import numpy as np \nfrom numba import jit\nfrom autolab_core import RigidTransform\n\n\ndef franka_pose_to_rigid_transform(franka_pose, from_frame='franka_tool_base', to_frame='world'):\n np_franka_pose = np.array(franka_pose).reshape(4, 4).T\n pose = RigidTransform(\n rotation=np_franka_pose[:3, :3], \n translation=np_franka_pose[:3, 3],\n from_frame=from_frame,\n to_frame=to_frame\n )\n return pose\n\n\ndef convert_rigid_transform_to_array(rigid_tf):\n return rigid_tf.matrix.reshape(-1)\n\n\ndef convert_array_to_rigid_transform(tf_array, from_frame='franka_tool', to_frame='world'):\n assert len(tf_array.shape) == 1 and tf_array.shape[0] == 16, \"Invalid array shape\"\n # Note that franka pose is in column format but this array is stored in normal \n # format hence we do not have do to any transposes.\n tf_mat = tf_array.reshape(4, 4)\n pose = RigidTransform(\n rotation=tf_mat[:3, :3], \n translation=tf_mat[:3, 3],\n from_frame=from_frame,\n to_frame=to_frame\n )\n return pose\n\n\n@jit(nopython=True)\ndef min_jerk_weight(t, T):\n r = t/T\n return (10 * r ** 3 - 15 * r ** 4 + 6 * r ** 5)\n\n\n@jit(nopython=True)\ndef min_jerk(xi, xf, t, T):\n return xi + (xf - xi) * min_jerk_weight(t, T)\n\n\n@jit(nopython=True)\ndef min_jerk_delta(xi, xf, t, T, dt):\n r = t/T\n return (xf - xi) * (30 * r ** 2 - 60 * r ** 3 + 30 * r ** 4) / T * dt\n\n\ndef transform_to_list(T):\n return T.matrix.T.flatten().tolist()\n" }, { "path": "launch/franka_rviz.launch", "content": "\n\n\n \n \n\n \n\n\n" }, { "path": "rviz/franka_basic_config.rviz", "content": "Panels:\n - Class: rviz/Displays\n Help Height: 78\n Name: Displays\n Property Tree Widget:\n Expanded:\n - /Global Options1\n - /Axes1\n Splitter Ratio: 0.5\n Tree Height: 775\n - Class: rviz/Selection\n Name: Selection\n - Class: rviz/Tool Properties\n Expanded:\n - /2D Pose Estimate1\n - /2D Nav Goal1\n - /Publish Point1\n Name: Tool Properties\n Splitter Ratio: 0.588679016\n - Class: rviz/Views\n Expanded:\n - /Current View1\n Name: Views\n Splitter Ratio: 0.5\n - Class: rviz/Time\n Experimental: false\n Name: Time\n SyncMode: 0\n SyncSource: \"\"\nToolbars:\n toolButtonStyle: 2\nVisualization Manager:\n Class: \"\"\n Displays:\n - Alpha: 0.5\n Cell Size: 1\n Class: rviz/Grid\n Color: 160; 160; 164\n Enabled: true\n Line Style:\n Line Width: 0.0299999993\n Value: Lines\n Name: Grid\n Normal Cell Count: 0\n Offset:\n X: 0\n Y: 0\n Z: 0\n Plane: XY\n Plane Cell Count: 10\n Reference Frame: \n Value: true\n - Alpha: 1\n Class: rviz/RobotModel\n Collision Enabled: false\n Enabled: true\n Links:\n All Links Enabled: true\n Expand Joint Details: false\n Expand Link Details: false\n Expand Tree: false\n Link Tree Style: Links in Alphabetic Order\n panda_hand:\n Alpha: 1\n Show Axes: false\n Show Trail: false\n Value: true\n panda_leftfinger:\n Alpha: 1\n Show Axes: false\n Show Trail: false\n Value: true\n panda_link0:\n Alpha: 1\n Show Axes: false\n Show Trail: false\n Value: true\n panda_link1:\n Alpha: 1\n Show Axes: false\n Show Trail: false\n Value: true\n panda_link2:\n Alpha: 1\n Show Axes: false\n Show Trail: false\n Value: true\n panda_link3:\n Alpha: 1\n Show Axes: false\n Show Trail: false\n Value: true\n panda_link4:\n Alpha: 1\n Show Axes: false\n Show Trail: false\n Value: true\n panda_link5:\n Alpha: 1\n Show Axes: false\n Show Trail: false\n Value: true\n panda_link6:\n Alpha: 1\n Show Axes: false\n Show Trail: false\n Value: true\n panda_link7:\n Alpha: 1\n Show Axes: false\n Show Trail: false\n Value: true\n panda_link8:\n Alpha: 1\n Show Axes: false\n Show Trail: false\n panda_rightfinger:\n Alpha: 1\n Show Axes: false\n Show Trail: false\n Value: true\n Name: RobotModel\n Robot Description: robot_description\n TF Prefix: \"\"\n Update Interval: 0\n Value: true\n Visual Enabled: true\n - Class: rviz/TF\n Enabled: false\n Frame Timeout: 15\n Frames:\n All Enabled: true\n Marker Scale: 1\n Name: TF\n Show Arrows: true\n Show Axes: true\n Show Names: true\n Tree:\n {}\n Update Interval: 0\n Value: false\n - Class: rviz/Axes\n Enabled: true\n Length: 0.100000001\n Name: Axes\n Radius: 0.00999999978\n Reference Frame: panda_end_effector\n Value: true\n Enabled: true\n Global Options:\n Background Color: 48; 48; 48\n Default Light: true\n Fixed Frame: panda_link0\n Frame Rate: 30\n Name: root\n Tools:\n - Class: rviz/Interact\n Hide Inactive Objects: true\n - Class: rviz/MoveCamera\n - Class: rviz/Select\n - Class: rviz/FocusCamera\n - Class: rviz/Measure\n - Class: rviz/SetInitialPose\n Topic: /initialpose\n - Class: rviz/SetGoal\n Topic: /move_base_simple/goal\n - Class: rviz/PublishPoint\n Single click: true\n Topic: /clicked_point\n Value: true\n Views:\n Current:\n Class: rviz/Orbit\n Distance: 2.24045944\n Enable Stereo Rendering:\n Stereo Eye Separation: 0.0599999987\n Stereo Focal Distance: 1\n Swap Stereo Eyes: false\n Value: false\n Focal Point:\n X: 0\n Y: 0\n Z: 0\n Focal Shape Fixed Size: true\n Focal Shape Size: 0.0500000007\n Invert Z Axis: false\n Name: Current View\n Near Clip Distance: 0.00999999978\n Pitch: 0.485398054\n Target Frame: \n Value: Orbit (rviz)\n Yaw: 0.0903962478\n Saved: ~\nWindow Geometry:\n Displays:\n collapsed: false\n Height: 1056\n Hide Left Dock: false\n Hide Right Dock: false\n QMainWindow State: 000000ff00000000fd00000004000000000000016a00000396fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000396000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000396fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000002800000396000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073f0000003efc0100000002fb0000000800540069006d006501000000000000073f0000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000004ba0000039600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000\n Selection:\n collapsed: false\n Time:\n collapsed: false\n Tool Properties:\n collapsed: false\n Views:\n collapsed: false\n Width: 1855\n X: 65\n Y: 24\n" }, { "path": "scripts/close_gripper.py", "content": "from frankapy import FrankaArm\n\n\nif __name__ == '__main__':\n fa = FrankaArm()\n fa.close_gripper()" }, { "path": "scripts/open_gripper.py", "content": "from frankapy import FrankaArm\n\n\nif __name__ == '__main__':\n fa = FrankaArm()\n fa.open_gripper()" }, { "path": "scripts/record_trajectory.py", "content": "import argparse\nimport time\nfrom frankapy import FrankaArm\nimport pickle as pkl\nimport numpy as np\n\nfrom frankapy.utils import convert_rigid_transform_to_array\n\n\ndef create_formated_skill_dict(joints, end_effector_positions, time_since_skill_started):\n skill_dict = dict(skill_description='GuideMode', skill_state_dict=dict())\n skill_dict['skill_state_dict']['q'] = np.array(joints)\n skill_dict['skill_state_dict']['O_T_EE'] = np.array(end_effector_positions)\n skill_dict['skill_state_dict']['time_since_skill_started'] = np.array(time_since_skill_started)\n\n # The key (0 here) usually represents the absolute time when the skill was started but\n formatted_dict = {0: skill_dict}\n return formatted_dict\n\n\nif __name__ == '__main__':\n parser = argparse.ArgumentParser()\n parser.add_argument('--time', '-t', type=float, default=10)\n parser.add_argument('--open_gripper', '-o', action='store_true')\n parser.add_argument('--file', '-f', default='franka_traj.pkl')\n args = parser.parse_args()\n\n print('Starting robot')\n fa = FrankaArm()\n if args.open_gripper:\n fa.open_gripper()\n\n print('Applying 0 force torque control for {}s'.format(args.time))\n end_effector_position = []\n joints = []\n\n time_since_skill_started = []\n fa.run_guide_mode(args.time, block=False)\n start_time = time.time()\n last_time = None\n\n while last_time is None or (last_time - start_time) < args.time:\n pose_array = convert_rigid_transform_to_array(fa.get_pose())\n end_effector_position.append(pose_array)\n joints.append(fa.get_joints())\n time_since_skill_started.append(time.time() - start_time)\n # add sleep to record at 100Hz\n time.sleep(0.0099)\n last_time = time.time()\n\n skill_dict = create_formated_skill_dict(joints, end_effector_position, time_since_skill_started)\n with open(args.file, 'wb') as pkl_f:\n pkl.dump(skill_dict, pkl_f)\n print(\"Did save skill dict: {}\".format(args.file))\n" }, { "path": "scripts/reset_arm.py", "content": "import argparse\nfrom frankapy import FrankaArm\n\nif __name__ == '__main__':\n parser = argparse.ArgumentParser()\n parser.add_argument('--use_pose', '-u', action='store_true')\n parser.add_argument('--close_grippers', '-c', action='store_true')\n args = parser.parse_args()\n\n print('Starting robot')\n fa = FrankaArm()\n\n if args.use_pose:\n print('Reset with pose')\n fa.reset_pose()\n else:\n print('Reset with joints')\n fa.reset_joints()\n \n if args.close_grippers:\n print('Closing Grippers')\n fa.close_gripper()\n else:\n print('Opening Grippers')\n fa.open_gripper()" }, { "path": "scripts/run_guide_mode.py", "content": "import argparse\nfrom frankapy import FrankaArm\n\nif __name__ == '__main__':\n parser = argparse.ArgumentParser()\n parser.add_argument('--time', '-t', type=float, default=100)\n parser.add_argument('--open_gripper', '-o', action='store_true')\n args = parser.parse_args()\n\n print('Starting robot')\n fa = FrankaArm()\n if args.open_gripper:\n fa.open_gripper()\n print('Applying 0 force torque control for {}s'.format(args.time))\n fa.run_guide_mode(args.time)\n print(fa.get_pose())\n print(fa.get_joints())" }, { "path": "scripts/run_guide_mode_ee_frame.py", "content": "import argparse\nfrom frankapy import FrankaArm\n\nif __name__ == '__main__':\n parser = argparse.ArgumentParser()\n parser.add_argument('--time', '-t', type=float, default=100)\n parser.add_argument('--open_gripper', '-o', action='store_true')\n args = parser.parse_args()\n\n print('Starting robot')\n fa = FrankaArm()\n if args.open_gripper:\n fa.open_gripper()\n fa.selective_guidance_mode(args.time, use_impedance=True, use_ee_frame=True, \n cartesian_impedances=[600.0, 0.0, 0.0, 0.0, 0.0, 0.0])" }, { "path": "scripts/run_recorded_trajectory.py", "content": "import argparse\nimport pickle\nimport numpy as np\nimport time\n\nfrom autolab_core import RigidTransform\nfrom frankapy import FrankaArm, SensorDataMessageType\nfrom frankapy import FrankaConstants as FC\nfrom frankapy.proto_utils import sensor_proto2ros_msg, make_sensor_group_msg\nfrom frankapy.proto import PosePositionSensorMessage, ShouldTerminateSensorMessage\nfrom franka_interface_msgs.msg import SensorDataGroup\n\nfrom frankapy.utils import convert_array_to_rigid_transform\n\nimport rospy\n\n\nif __name__ == \"__main__\":\n parser = argparse.ArgumentParser()\n parser.add_argument('--trajectory_pickle', '-t', type=str, required=True,\n help='Path to trajectory (in pickle format) to replay.')\n parser.add_argument('--open_gripper', '-o', action='store_true')\n args = parser.parse_args()\n\n print('Starting robot')\n fa = FrankaArm()\n fa.reset_joints()\n\n rospy.loginfo('Loading Trajectory')\n\n with open(args.trajectory_pickle, 'rb') as pkl_f:\n skill_data = pickle.load(pkl_f)\n \n assert skill_data[0]['skill_description'] == 'GuideMode', \\\n \"Trajectory not collected in guide mode\"\n skill_state_dict = skill_data[0]['skill_state_dict']\n\n T = float(skill_state_dict['time_since_skill_started'][-1])\n dt = 0.01\n ts = np.arange(0, T, dt)\n\n pose_traj = skill_state_dict['O_T_EE']\n # Goto the first position in the trajectory.\n print(convert_array_to_rigid_transform(pose_traj[0]).matrix)\n fa.goto_pose(convert_array_to_rigid_transform(pose_traj[0]), \n duration=4.0, \n cartesian_impedances=[600.0, 600.0, 600.0, 50.0, 50.0, 50.0])\n \n rospy.loginfo('Initializing Sensor Publisher')\n pub = rospy.Publisher(FC.DEFAULT_SENSOR_PUBLISHER_TOPIC, SensorDataGroup, queue_size=10)\n rate = rospy.Rate(1 / dt)\n\n rospy.loginfo('Publishing pose trajectory...')\n # To ensure skill doesn't end before completing trajectory, make the buffer time much longer than needed\n fa.goto_pose(convert_array_to_rigid_transform(pose_traj[1]), \n duration=T, \n dynamic=True, \n buffer_time=10, \n cartesian_impedances=[600.0, 600.0, 600.0, 50.0, 50.0, 50.0]\n )\n init_time = rospy.Time.now().to_time()\n for i in range(2, len(ts)):\n timestamp = rospy.Time.now().to_time() - init_time\n pose_tf = convert_array_to_rigid_transform(pose_traj[i])\n traj_gen_proto_msg = PosePositionSensorMessage(\n id=i, \n timestamp=timestamp,\n position=pose_tf.translation, \n quaternion=pose_tf.quaternion\n\t\t)\n ros_msg = make_sensor_group_msg(\n trajectory_generator_sensor_msg=sensor_proto2ros_msg(\n traj_gen_proto_msg, \n SensorDataMessageType.POSE_POSITION),\n )\n\n # Sleep the same amount as the trajectory was recorded in\n dt = skill_state_dict['time_since_skill_started'][i] - skill_state_dict['time_since_skill_started'][i-1]\n rospy.loginfo('Publishing: ID {}, dt: {:.4f}'.format(traj_gen_proto_msg.id, dt))\n pub.publish(ros_msg)\n time.sleep(dt)\n # Finished trajectory\n if i >= pose_traj.shape[0] - 1:\n break\n\n # Stop the skill\n # Alternatively can call fa.stop_skill()\n term_proto_msg = ShouldTerminateSensorMessage(timestamp=rospy.Time.now().to_time() - init_time, \n should_terminate=True)\n ros_msg = make_sensor_group_msg(\n termination_handler_sensor_msg=sensor_proto2ros_msg(\n term_proto_msg, SensorDataMessageType.SHOULD_TERMINATE)\n )\n pub.publish(ros_msg)\n\n rospy.loginfo('Done')\n" }, { "path": "scripts/test_virtual_walls.py", "content": "from frankapy import FrankaArm\n\nif __name__ == '__main__':\n print('Starting robot')\n fa = FrankaArm()\n\n fa.reset_joints()\n\n pose = fa.get_pose()\n pose.translation[0] = 0.75\n\n # This should trigger an error\n fa.goto_pose(pose)\n " }, { "path": "setup.py", "content": "\"\"\"\nFrankaPy Franka Panda Robot Control Library\n\"\"\"\nfrom setuptools import setup\n\nrequirements = [\n 'autolab_core',\n 'empy==3.3.4',\n 'numpy',\n 'numpy-quaternion',\n 'numba',\n 'rospkg',\n 'catkin-tools',\n 'protobuf'\n]\n\nsetup(name='frankapy',\n version='1.0.0',\n description='FrankaPy Franka Panda Robot Control Library',\n author='Kevin Zhang, Mohit Sharma, Jacky Liang, Oliver Kroemer',\n author_email='',\n package_dir = {'': '.'},\n packages=['frankapy'],\n install_requires = requirements,\n extras_require = {}\n )\n" }, { "path": "urdf/hand.urdf.xacro", "content": "\n\n \n \n\n" }, { "path": "urdf/hand.xacro", "content": "\n\n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n\n" }, { "path": "urdf/panda_arm.urdf.xacro", "content": "\n\n \n \n\n" }, { "path": "urdf/panda_arm.xacro", "content": "\n\n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n\n" }, { "path": "urdf/panda_arm_hand.urdf.xacro", "content": "\n\n \n \n \n \n\n" } ]