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