├── .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 ├── imgs │ └── network_config.png ├── 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 /.github/workflows/docs_pages_workflow.yml: -------------------------------------------------------------------------------- 1 | name: docs_pages_workflow 2 | 3 | # execute this workflow automatically when a we push to master 4 | on: 5 | push: 6 | branches: [ master ] 7 | 8 | jobs: 9 | 10 | build_docs_job: 11 | runs-on: ubuntu-latest 12 | container: ubuntu:focal 13 | 14 | steps: 15 | 16 | - name: Prereqs 17 | env: 18 | GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }} 19 | run: | 20 | apt-get update 21 | apt-get install -y git 22 | git clone --depth 1 "https://token:${GITHUB_TOKEN}@github.com/${GITHUB_REPOSITORY}.git" . 23 | shell: bash 24 | 25 | - name: Setup ROS environment 26 | uses: ros-tooling/setup-ros@0.2.1 27 | with: 28 | required-ros-distributions: noetic 29 | 30 | - name: Execute script to build our documentation and update pages 31 | env: 32 | GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }} 33 | run: "docs/buildDocs.sh" 34 | shell: bash 35 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # Byte-compiled / optimized / DLL files 2 | __pycache__/ 3 | *.py[cod] 4 | *$py.class 5 | 6 | # C extensions 7 | *.so 8 | 9 | # Distribution / packaging 10 | .Python 11 | build/ 12 | develop-eggs/ 13 | dist/ 14 | downloads/ 15 | eggs/ 16 | .eggs/ 17 | lib/ 18 | lib64/ 19 | parts/ 20 | sdist/ 21 | var/ 22 | wheels/ 23 | pip-wheel-metadata/ 24 | share/python-wheels/ 25 | *.egg-info/ 26 | .installed.cfg 27 | *.egg 28 | MANIFEST 29 | 30 | # PyInstaller 31 | # Usually these files are written by a python script from a template 32 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 33 | *.manifest 34 | *.spec 35 | 36 | # Installer logs 37 | pip-log.txt 38 | pip-delete-this-directory.txt 39 | 40 | # Unit test / coverage reports 41 | htmlcov/ 42 | .tox/ 43 | .nox/ 44 | .coverage 45 | .coverage.* 46 | .cache 47 | nosetests.xml 48 | coverage.xml 49 | *.cover 50 | *.py,cover 51 | .hypothesis/ 52 | .pytest_cache/ 53 | 54 | # Translations 55 | *.mo 56 | *.pot 57 | 58 | # Django stuff: 59 | *.log 60 | local_settings.py 61 | db.sqlite3 62 | db.sqlite3-journal 63 | 64 | # Flask stuff: 65 | instance/ 66 | .webassets-cache 67 | 68 | # Scrapy stuff: 69 | .scrapy 70 | 71 | # PyBuilder 72 | target/ 73 | 74 | # Jupyter Notebook 75 | .ipynb_checkpoints 76 | 77 | # IPython 78 | profile_default/ 79 | ipython_config.py 80 | 81 | # pyenv 82 | .python-version 83 | 84 | # pipenv 85 | # According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control. 86 | # However, in case of collaboration, if having platform-specific dependencies or dependencies 87 | # having no cross-platform support, pipenv may install dependencies that don't work, or not 88 | # install all needed dependencies. 89 | #Pipfile.lock 90 | 91 | # PEP 582; used by e.g. github.com/David-OConnor/pyflow 92 | __pypackages__/ 93 | 94 | # Celery stuff 95 | celerybeat-schedule 96 | celerybeat.pid 97 | 98 | # SageMath parsed files 99 | *.sage.py 100 | 101 | # Environments 102 | .env 103 | .venv 104 | env/ 105 | venv/ 106 | ENV/ 107 | env.bak/ 108 | venv.bak/ 109 | 110 | # Spyder project settings 111 | .spyderproject 112 | .spyproject 113 | 114 | # Rope project settings 115 | .ropeproject 116 | 117 | # mkdocs documentation 118 | /site 119 | 120 | # mypy 121 | .mypy_cache/ 122 | .dmypy.json 123 | dmypy.json 124 | 125 | # Pyre type checker 126 | .pyre/ 127 | 128 | # ROS 129 | catkin_ws/devel 130 | catkin_ws/build 131 | catkin_ws/logs 132 | catkin_ws/.catkin_tools -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "catkin_ws/src/franka-interface-msgs"] 2 | path = catkin_ws/src/franka-interface-msgs 3 | url = https://github.com/iamlab-cmu/franka-interface-msgs.git 4 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2021 Carnegie Mellon University 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # frankapy 2 | 3 | This is a software package used for controlling and learning skills on the Franka Emika Panda Research Robot Arm. 4 | 5 | Installation Instructions and Network Configuration Instructions are also available here: [https://iamlab-cmu.github.io/frankapy](https://iamlab-cmu.github.io/frankapy) 6 | 7 | To join the Discord community, click the link [here](https://discord.gg/r6r7dttMwZ). 8 | 9 | ## Requirements 10 | 11 | * A computer with the Ubuntu 18.04 / 20.04. 12 | * ROS Melodic / Noetic 13 | * [Protocol Buffers](https://github.com/protocolbuffers/protobuf) 14 | 15 | ## Computer Setup Instructions 16 | 17 | 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). 18 | `FrankaPy` will send commands to [franka-interface](https://github.com/iamlab-cmu/franka-interface), which actually controls the robot. 19 | 20 | ## Install ProtoBuf 21 | 22 | **This is only needed if you plan to modify the proto messages. You don't need to install or compile protobuf for using frankapy** 23 | 24 | We use both C++ and Python versions of protobufs so you would need to install Protobufs from source. 25 | 26 | Do `nproc` to find out how many cores you have, and use that as the `N` number in the `make` command below: 27 | 28 | ```shell 29 | sudo apt-get install autoconf automake libtool curl make g++ unzip 30 | wget https://github.com/protocolbuffers/protobuf/releases/download/v21.8/protobuf-all-21.8.zip 31 | unzip protobuf-all-21.8.zip 32 | cd protobuf-21.8 33 | ./configure 34 | make -jN 35 | make check -jN 36 | sudo make install 37 | sudo ldconfig 38 | ``` 39 | See detailed instructions [here](https://github.com/protocolbuffers/protobuf/blob/master/src/README.md) 40 | 41 | ## Installation 42 | 43 | 1. Clone Repo and its Submodules: 44 | 45 | ```bash 46 | git clone --recurse-submodules git@github.com:iamlab-cmu/frankapy.git 47 | ``` 48 | All directories below are given relative to `/frankapy`. 49 | 50 | 2. First source into your virtualenv or conda env (should be Python 3.6). Then: 51 | ```bash 52 | pip install -e . 53 | ``` 54 | 55 | 3. To compile the catkin_ws use the following script: 56 | ```bash 57 | ./bash_scripts/make_catkin.sh 58 | ``` 59 | 60 | 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: 61 | ```bash 62 | sudo apt install ros-melodic-libfranka ros-melodic-franka-ros 63 | ``` 64 | 65 | 5. To make the protobufs use the following script (**you don't need to do this if you haven't modified the proto messages**): 66 | ```bash 67 | ./bash_scripts/make_proto.sh 68 | ``` 69 | 70 | ## Configuring the network with the Control PC 71 | ### Ethernet 72 | 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. 73 | 2. Select the Ethernet connection that corresponds to the port that you plugged the ethernet cable into and then click edit. 74 | 3. Go to the IPv4 Settings Tab and switch from Automatic (DHCP) to Manual. 75 | 4. Add a static ip address like 192.168.1.3 on the Control PC with netmask 24 and then click save. 76 | 5. Then do the same on the FrankaPy PC but instead set the static ip address to be 192.168.1.2. 77 | 78 | ### Wifi 79 | 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. 80 | 81 | ### Editing the /etc/hosts file 82 | 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. 83 | 2. On the Control PC, run the command: `sudo gedit /etc/hosts` 84 | 3. If you are using an Ethernet connection, then add the following above the line `# The following lines are desirable for IPv6 capable hosts`: 85 | ```bash 86 | 192.168.1.2 [frankapy-pc-name] 87 | ``` 88 | Otherwise substitute 192.168.1.2 with the ip address of the FrankaPy PC that you discovered using the command `ifconfig`. 89 | 4. Afterwards, on the FrankaPy PC, again run the command `sudo gedit /etc/hosts` and add the line: 90 | ```bash 91 | 192.168.1.3 [control-pc-name] 92 | ``` 93 | Otherwise substitute 192.168.1.3 with the ip address of the Control PC that you discovered using the command `ifconfig`. 94 | 5. Now you should be able to ssh between the FrankaPy PC and the Control PC using the command: 95 | ```bash 96 | ssh [control-pc-username]@[control-pc-name] 97 | Input password to control-pc. 98 | ``` 99 | 100 | ## Setting Up SSH Key to Control PC 101 | 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 102 | ```bash 103 | ssh-keygen -t rsa -b 4096 -C "your_email@example.com" 104 | [Press enter] 105 | [Press enter] 106 | [Press enter] 107 | eval "$(ssh-agent -s)" 108 | ssh-add ~/.ssh/id_rsa 109 | ``` 110 | 2. Upload your public ssh key to the control pc. 111 | 1. In a new terminal, ssh to the control PC. 112 | ```bash 113 | ssh [control-pc-username]@[control-pc-name] 114 | Input password to control-pc. 115 | ``` 116 | 2. Use your favorite text editor to open the authorized_keys file. 117 | ```bash 118 | vim ~/.ssh/authorized_keys 119 | ``` 120 | 3. In a separate terminal on your Workhorse PC, use your favorite text editor to open your id_rsa.pub file. 121 | ```bash 122 | vim ~/.ssh/id_rsa.pub 123 | ``` 124 | 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. 125 | 5. Open a new terminal and try sshing to the control PC and it should no longer require a password. 126 | 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 127 | 128 | ## Unlocking the Franka Robot 129 | 1. In a new terminal, ssh to the control PC with option -X. 130 | ```bash 131 | ssh -X [control-pc-username]@[control-pc-name] 132 | ``` 133 | 2. Open a web browser, either firefox or google chrome. 134 | ```bash 135 | firefox 136 | ``` 137 | 3. Go to 172.16.0.2 in the web browser. 138 | 4. (Optional) Input the username admin and the password to login to the Franka Desk GUI. 139 | 5. Unlock the robot by clicking the unlock button on the bottom right of the web interface. 140 | 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. 141 | 142 | ## Running the Franka Robot 143 | 144 | 1. Make sure that both the user stop and the brakes of the Franka robot have been unlocked in the Franka Desk GUI. 145 | 2. Open up a new terminal and go to the frankapy directory. 146 | ```bash 147 | bash ./bash_scripts/start_control_pc.sh -i [control-pc-name] 148 | ``` 149 | 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`. 150 | 151 | 3. Open up a new terminal and go to the frankapy directory. Do: 152 | ```bash 153 | source catkin_ws/devel/setup.bash 154 | ``` 155 | 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. 156 | 157 | ```bash 158 | python scripts/reset_arm.py 159 | ``` 160 | 161 | See example scripts in the `examples/` and `scripts/` to learn how to use the `FrankaPy` python package. 162 | 163 | ## Citation 164 | 165 | If this library proves useful to your research, please cite the paper below:: 166 | ``` 167 | @article{zhang2020modular, 168 | title={A modular robotic arm control stack for research: Franka-interface and frankapy}, 169 | author={Zhang, Kevin and Sharma, Mohit and Liang, Jacky and Kroemer, Oliver}, 170 | journal={arXiv preprint arXiv:2011.02398}, 171 | year={2020} 172 | } 173 | ``` 174 | -------------------------------------------------------------------------------- /bash_scripts/make_catkin.sh: -------------------------------------------------------------------------------- 1 | cd catkin_ws 2 | catkin build 3 | cd .. -------------------------------------------------------------------------------- /bash_scripts/make_proto.sh: -------------------------------------------------------------------------------- 1 | protoc -I=frankapy/proto/ --python_out=frankapy/proto/ frankapy/proto/*.proto -------------------------------------------------------------------------------- /bash_scripts/start_control_pc.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | die () { 4 | echo >&2 "$@" 5 | exit 1 6 | } 7 | 8 | usage="$(basename "$0") [-h] [-i xxx.xxx.xxx.xxx] -- Start control PC from workstation 9 | 10 | where: 11 | -h show this help text 12 | -i IP address for the Control PC. If you use localhost, it will treat the current PC as the Control PC. 13 | -u Username on control PC. (default iam-lab) 14 | -p Control PC password 15 | -d Path to franka_interface on Control PC (default Documents/franka-interface) 16 | -r Robot number (default 1) 17 | -a Robot IP address (default 172.16.0.2) 18 | -s Start franka-interface on Control PC (0 / 1 (default)) 19 | -g Robot has gripper (0 / 1 (default)) 20 | -o Using old gripper commands (0 (default) / 1) 21 | -l Log at 1kHz on franka-interface (0 (default) / 1) 22 | -e Stop franka-interface when an error has occurred (0 (default) / 1) 23 | 24 | ./start_control_pc.sh -i iam-space 25 | ./start_control_pc.sh -i iam-space -u iam-lab -p 12345678 -d ~/Documents/franka-interface -r 1 -s 0 26 | " 27 | 28 | control_pc_uname="iam-lab" 29 | control_pc_use_password=0 30 | control_pc_password="" 31 | control_pc_franka_interface_path="Documents/franka-interface" 32 | start_franka_interface=1 33 | robot_number=1 34 | robot_ip="172.16.0.2" 35 | with_gripper=1 36 | old_gripper=0 37 | log_on_franka_interface=0 38 | stop_on_error=0 39 | 40 | while getopts ':h:i:u:p:d:r:a:s:g:o:l:e' option; do 41 | case "${option}" in 42 | h) echo "$usage" 43 | exit 44 | ;; 45 | i) control_pc_ip_address=$OPTARG 46 | ;; 47 | u) control_pc_uname=$OPTARG 48 | ;; 49 | p) control_pc_password=$OPTARG 50 | control_pc_use_password=1 51 | ;; 52 | d) control_pc_franka_interface_path=$OPTARG 53 | ;; 54 | r) robot_number=$OPTARG 55 | ;; 56 | a) robot_ip=$OPTARG 57 | ;; 58 | s) start_franka_interface=$OPTARG 59 | ;; 60 | g) with_gripper=$OPTARG 61 | ;; 62 | o) old_gripper=$OPTARG 63 | ;; 64 | l) log_on_franka_interface=$OPTARG 65 | ;; 66 | e) stop_on_error=$OPTARG 67 | ;; 68 | :) printf "missing argument for -%s\n" "$OPTARG" >&2 69 | echo "$usage" >&2 70 | exit 1 71 | ;; 72 | \?) printf "illegal option: -%s\n" "$OPTARG" >&2 73 | echo "$usage" >&2 74 | exit 1 75 | ;; 76 | esac 77 | done 78 | shift $((OPTIND - 1)) 79 | 80 | workstation_ip_address="`hostname`" 81 | 82 | # Notify the IP addresses being used. 83 | echo "Control PC IP uname/address: "$control_pc_uname"@"$control_pc_ip_address 84 | echo "Workstation IP address: "$workstation_ip_address 85 | if [ "$control_pc_use_password" -eq 0 ]; then 86 | echo "Will not use password to ssh into control pc." 87 | else 88 | echo "Will use input password to ssh into control pc." 89 | fi 90 | 91 | DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )" 92 | 93 | # Start rosmaster in a new gnome-terminal if not already running 94 | if ! pgrep -x "roscore" > /dev/null 95 | then 96 | start_rosmaster_path="$DIR/start_rosmaster.sh" 97 | echo "Will start ROS master in new terminal."$start_rosmaster_path 98 | gnome-terminal --working-directory="$DIR" -- bash $start_rosmaster_path 99 | sleep 3 100 | echo "Did start ROS master in new terminal." 101 | else 102 | echo "Roscore is already running" 103 | fi 104 | 105 | if [ "$with_gripper" -eq 0 ]; then 106 | let old_gripper=0 107 | fi 108 | 109 | if [ "$start_franka_interface" -eq 1 ]; then 110 | # ssh to the control pc and start franka_interface in a new gnome-terminal 111 | start_franka_interface_on_control_pc_path="$DIR/start_franka_interface_on_control_pc.sh" 112 | echo "Will ssh to control PC and start franka-interface." 113 | 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 114 | echo "Done" 115 | sleep 3 116 | else 117 | echo "Will not start franka-interface on the control pc." 118 | fi 119 | 120 | # ssh to the control pc and start ROS action server in a new gnome-terminal 121 | start_franka_ros_interface_on_control_pc_path="$DIR/start_franka_ros_interface_on_control_pc.sh" 122 | echo "Will ssh to control PC and start ROS action server." 123 | 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 124 | sleep 3 125 | 126 | if [ "$with_gripper" -eq 1 ] && [ "$old_gripper" -eq 0 ]; then 127 | start_franka_gripper_on_control_pc_path="$DIR/start_franka_gripper_on_control_pc.sh" 128 | echo "Will ssh to control PC and start ROS action server." 129 | 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 130 | sleep 3 131 | else 132 | echo "Will not start franka gripper on the control pc." 133 | fi 134 | 135 | echo "Done" 136 | -------------------------------------------------------------------------------- /bash_scripts/start_franka_gripper_on_control_pc.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | control_pc_uname=${1} 4 | control_pc_ip_address=${2} 5 | workstation_ip_address=${3} 6 | control_pc_franka_interface_path=${4} 7 | robot_number=${5} 8 | robot_ip=${6} 9 | control_pc_use_password=${7} 10 | control_pc_password=${8} 11 | 12 | rosmaster_path="bash_scripts/set_rosmaster.sh" 13 | catkin_ws_setup_path="catkin_ws/devel/setup.bash" 14 | 15 | if [ "$control_pc_ip_address" = "localhost" ]; then 16 | cd $HOME 17 | cd $control_pc_franka_interface_path 18 | source $catkin_ws_setup_path 19 | roslaunch franka_ros_interface franka_gripper.launch robot_num:=$robot_number robot_ip:=$robot_ip 20 | bash 21 | else 22 | if [ "$control_pc_use_password" = "0" ]; then 23 | ssh -tt $control_pc_uname@$control_pc_ip_address << EOSSH 24 | cd $control_pc_franka_interface_path 25 | source $rosmaster_path $control_pc_ip_address $workstation_ip_address 26 | source $catkin_ws_setup_path 27 | roslaunch franka_ros_interface franka_gripper.launch robot_num:=$robot_number robot_ip:=$robot_ip 28 | bash 29 | EOSSH 30 | else 31 | sshpass -p "$control_pc_password" ssh -tt -o StrictHostKeyChecking=no $control_pc_uname@$control_pc_ip_address << EOSSH 32 | cd $control_pc_franka_interface_path 33 | source $rosmaster_path $control_pc_ip_address $workstation_ip_address 34 | source $catkin_ws_setup_path 35 | roslaunch franka_ros_interface franka_gripper.launch robot_num:=$robot_number robot_ip:=$robot_ip 36 | bash 37 | EOSSH 38 | fi 39 | fi -------------------------------------------------------------------------------- /bash_scripts/start_franka_interface_on_control_pc.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | robot_ip=${1} 4 | with_gripper=${2} 5 | log_on_franka_interface=${3} 6 | stop_on_error=${4} 7 | control_pc_uname=${5} 8 | control_pc_ip_address=${6} 9 | control_pc_franka_interface_path=${7} 10 | control_pc_use_password=${8} 11 | control_pc_password=${9} 12 | franka_interface_stop_on_error=${10} 13 | 14 | 15 | if [ "$control_pc_ip_address" = "localhost" ]; then 16 | cd $HOME 17 | cd $control_pc_franka_interface_path 18 | cd build 19 | ./franka_interface --robot_ip $robot_ip --with_gripper $with_gripper --log $log_on_franka_interface --stop_on_error $stop_on_error 20 | bash 21 | else 22 | if [ "$control_pc_use_password" = "0" ]; then 23 | ssh -tt $control_pc_uname@$control_pc_ip_address << EOSSH 24 | cd $control_pc_franka_interface_path 25 | cd build 26 | ./franka_interface --robot_ip $robot_ip --with_gripper $with_gripper --log $log_on_franka_interface --stop_on_error $stop_on_error 27 | bash 28 | EOSSH 29 | else 30 | sshpass -p "$control_pc_password" ssh -tt -o StrictHostKeyChecking=no $control_pc_uname@$control_pc_ip_address << EOSSH 31 | cd $control_pc_franka_interface_path 32 | cd build 33 | echo $stop_on_error 34 | ./franka_interface --robot_ip $robot_ip --with_gripper $with_gripper --log $log_on_franka_interface --stop_on_error $stop_on_error 35 | bash 36 | EOSSH 37 | fi 38 | fi -------------------------------------------------------------------------------- /bash_scripts/start_franka_ros_interface_on_control_pc.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | control_pc_uname=${1} 4 | control_pc_ip_address=${2} 5 | workstation_ip_address=${3} 6 | control_pc_franka_interface_path=${4} 7 | robot_number=${5} 8 | control_pc_use_password=${6} 9 | control_pc_password=${7} 10 | 11 | rosmaster_path="bash_scripts/set_rosmaster.sh" 12 | catkin_ws_setup_path="catkin_ws/devel/setup.bash" 13 | 14 | if [ "$control_pc_ip_address" = "localhost" ]; then 15 | cd $HOME 16 | cd $control_pc_franka_interface_path 17 | source $catkin_ws_setup_path 18 | roslaunch franka_ros_interface franka_ros_interface.launch robot_num:=$robot_number 19 | bash 20 | else 21 | if [ "$control_pc_use_password" = "0" ]; then 22 | ssh -tt $control_pc_uname@$control_pc_ip_address << EOSSH 23 | cd $control_pc_franka_interface_path 24 | source $rosmaster_path $control_pc_ip_address $workstation_ip_address 25 | source $catkin_ws_setup_path 26 | roslaunch franka_ros_interface franka_ros_interface.launch robot_num:=$robot_number 27 | bash 28 | EOSSH 29 | else 30 | sshpass -p "$control_pc_password" ssh -tt -o StrictHostKeyChecking=no $control_pc_uname@$control_pc_ip_address << EOSSH 31 | cd $control_pc_franka_interface_path 32 | source $rosmaster_path $control_pc_ip_address $workstation_ip_address 33 | source $catkin_ws_setup_path 34 | roslaunch franka_ros_interface franka_ros_interface.launch robot_num:=$robot_number 35 | bash 36 | EOSSH 37 | fi 38 | fi 39 | -------------------------------------------------------------------------------- /bash_scripts/start_rosmaster.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | roscore -------------------------------------------------------------------------------- /cfg/april_tag_pick_place_cfg.yaml: -------------------------------------------------------------------------------- 1 | rs: 2 | id: 0 3 | frame: realsense 4 | filter_depth: True 5 | 6 | april_tag: 7 | detector: 8 | families: tag36h11 9 | border: 1 10 | nthreads: 4 11 | quad_decimate: 1.0 12 | quad_blur: 0.0 13 | refine_edges: True 14 | refine_decode: False 15 | refine_pose: False 16 | debug: False 17 | quad_contours: True 18 | tag_size: 0.0254 # 1 inch in meters 19 | 20 | T_camera_ee_path: examples/T_RSD415_franka.tf 21 | T_camera_mount_path: examples/T_RS_mount_delta.tf 22 | 23 | cube_size: 0.04 24 | 25 | vis_detect: True 26 | -------------------------------------------------------------------------------- /docs/.gitignore: -------------------------------------------------------------------------------- 1 | *.swp 2 | /_build 3 | /doctrees 4 | -------------------------------------------------------------------------------- /docs/Makefile: -------------------------------------------------------------------------------- 1 | # Minimal makefile for Sphinx documentation 2 | # 3 | 4 | # You can set these variables from the command line, and also 5 | # from the environment for the first two. 6 | SPHINXOPTS ?= 7 | SPHINXBUILD ?= sphinx-build 8 | SOURCEDIR = . 9 | BUILDDIR = _build 10 | 11 | # Put it first so that "make" without argument is like "make help". 12 | help: 13 | @$(SPHINXBUILD) -M help "$(SOURCEDIR)" "$(BUILDDIR)" $(SPHINXOPTS) $(O) 14 | 15 | .PHONY: help Makefile 16 | 17 | # Catch-all target: route all unknown targets to Sphinx using the new 18 | # "make mode" option. $(O) is meant as a shortcut for $(SPHINXOPTS). 19 | %: Makefile 20 | @$(SPHINXBUILD) -M $@ "$(SOURCEDIR)" "$(BUILDDIR)" $(SPHINXOPTS) $(O) 21 | -------------------------------------------------------------------------------- /docs/buildDocs.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | set -x 3 | ################################################################################ 4 | # File: buildDocs.sh 5 | # Purpose: Script that builds our documentation using sphinx and updates GitHub 6 | # Pages. This script is executed by: 7 | # .github/workflows/docs_pages_workflow.yml 8 | # 9 | # Authors: Michael Altfield 10 | # Created: 2020-07-17 11 | # Updated: 2020-07-17 12 | # Version: 0.1 13 | ################################################################################ 14 | 15 | ################### 16 | # INSTALL DEPENDS # 17 | ################### 18 | 19 | apt-get update 20 | apt-get -y install git rsync python3-sphinx python3-sphinx-rtd-theme python3-setuptools python3-pip 21 | apt-get -y install ros-noetic-libfranka ros-noetic-franka-ros 22 | 23 | sudo rosdep init 24 | rosdep update 25 | 26 | sudo -H pip3 install --upgrade pip 27 | 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 28 | sudo -H pip3 install . 29 | 30 | source /opt/ros/noetic/setup.bash 31 | 32 | git config --global --add safe.directory /__w/frankapy/frankapy 33 | git submodule update --init --recursive 34 | 35 | cd catkin_ws 36 | catkin build 37 | source devel/setup.bash 38 | cd .. 39 | 40 | ##################### 41 | # DECLARE VARIABLES # 42 | ##################### 43 | 44 | pwd 45 | ls -lah 46 | export SOURCE_DATE_EPOCH=$(git log -1 --pretty=%ct) 47 | 48 | ############## 49 | # BUILD DOCS # 50 | ############## 51 | 52 | # build our documentation with sphinx (see docs/conf.py) 53 | # * https://www.sphinx-doc.org/en/master/usage/quickstart.html#running-the-build 54 | make -C docs clean 55 | make -C docs html 56 | 57 | ####################### 58 | # Update GitHub Pages # 59 | ####################### 60 | 61 | git config --global user.name "${GITHUB_ACTOR}" 62 | git config --global user.email "${GITHUB_ACTOR}@users.noreply.github.com" 63 | 64 | docroot=`mktemp -d` 65 | rsync -av "docs/_build/html/" "${docroot}/" 66 | 67 | pushd "${docroot}" 68 | 69 | # don't bother maintaining history; just generate fresh 70 | git init 71 | git remote add deploy "https://token:${GITHUB_TOKEN}@github.com/${GITHUB_REPOSITORY}.git" 72 | git checkout -b gh-pages 73 | 74 | # add .nojekyll to the root so that github won't 404 on content added to dirs 75 | # that start with an underscore (_), such as our "_content" dir.. 76 | touch .nojekyll 77 | 78 | # Add README 79 | cat > README.md <`_. 12 | 13 | If this library proves useful to your research, please cite the paper below:: 14 | 15 | @article{zhang2020modular, 16 | title={A modular robotic arm control stack for research: Franka-interface and frankapy}, 17 | author={Zhang, Kevin and Sharma, Mohit and Liang, Jacky and Kroemer, Oliver}, 18 | journal={arXiv preprint arXiv:2011.02398}, 19 | year={2020} 20 | } 21 | 22 | Note that this library has been released with the Apache v2.0 license. 23 | 24 | .. toctree:: 25 | :maxdepth: 2 26 | 27 | install 28 | network 29 | running 30 | frankaarm 31 | support 32 | 33 | Indices and tables 34 | ================== 35 | 36 | * :ref:`genindex` 37 | * :ref:`modindex` 38 | * :ref:`search` 39 | -------------------------------------------------------------------------------- /docs/install.rst: -------------------------------------------------------------------------------- 1 | Installation 2 | ============ 3 | 4 | Requirements 5 | ------------ 6 | 7 | * A computer with Ubuntu 18.04 / 20.04 8 | * ROS Melodic / Noetic 9 | 10 | 11 | Steps 12 | ----- 13 | 14 | 1. Clone the Frankapy Repository and its Submodules:: 15 | 16 | git clone --recurse-submodules git@github.com:iamlab-cmu/frankapy.git 17 | 18 | 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):: 19 | 20 | sudo apt install ros-melodic-libfranka ros-melodic-franka-ros 21 | 22 | 3. Create and enter a virtual environment (:ref:`Virtual Environment`) and then run the following commands:: 23 | 24 | cd frankapy 25 | pip install -e . 26 | 27 | 4. Compile the catkin_ws using the following script:: 28 | 29 | ./bash_scripts/make_catkin.sh 30 | 31 | 5. Afterwards source the ``catkin_ws`` using the following command:: 32 | 33 | source catkin_ws/devel/setup.bash 34 | 35 | 6. It is a good idea to add the following line to the end of your ``~/.bashrc`` file:: 36 | 37 | source /path/to/frankapy/catkin_ws/devel/setup.bash --extend 38 | 39 | 40 | (Optional) Additional Steps 41 | --------------------------- 42 | 43 | Protobuf 44 | ~~~~~~~~ 45 | 46 | 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. 47 | 48 | 1. First determine the number of cores on your computer using the command:: 49 | 50 | nproc 51 | 52 | 2. Execute the following commands:: 53 | 54 | sudo apt-get install autoconf automake libtool curl make g++ unzip 55 | wget https://github.com/protocolbuffers/protobuf/releases/download/v21.8/protobuf-all-21.8.zip 56 | unzip protobuf-all-21.8.zip 57 | cd protobuf-21.8 58 | ./configure 59 | 60 | 3. Use the number that was previously printed out using the ``nproc`` command above and substitute it as ``N`` below:: 61 | 62 | make -jN 63 | sudo make install 64 | sudo ldconfig 65 | 66 | 4. Afterwards, you can make the protobuf messages using the following script:: 67 | 68 | ./bash_scripts/make_proto.sh 69 | 70 | 71 | Virtual Environment 72 | ~~~~~~~~~~~~~~~~~~~ 73 | 74 | Note that these instructions work on Ubuntu 18.04. They might be slightly different for Ubuntu 20.04. 75 | 76 | 1. Install Python3.6:: 77 | 78 | sudo apt install -y python3-distutils 79 | 80 | 2. Install Pip:: 81 | 82 | curl https://bootstrap.pypa.io/get-pip.py | sudo -H python3.6 83 | 84 | 3. Install Virtual Environment and Other Useful Python Packages:: 85 | 86 | sudo -H pip3.6 install numpy matplotlib virtualenv 87 | 88 | 4. Create a Virtual Environment for Frankapy:: 89 | 90 | virtualenv -p python3.6 franka 91 | 92 | 5. Enter into the Virtual Environment:: 93 | 94 | source franka/bin/activate 95 | 96 | 6. How to exit the Virtual Environment:: 97 | 98 | deactivate 99 | -------------------------------------------------------------------------------- /docs/make.bat: -------------------------------------------------------------------------------- 1 | @ECHO OFF 2 | 3 | pushd %~dp0 4 | 5 | REM Command file for Sphinx documentation 6 | 7 | if "%SPHINXBUILD%" == "" ( 8 | set SPHINXBUILD=sphinx-build 9 | ) 10 | set SOURCEDIR=. 11 | set BUILDDIR=_build 12 | 13 | if "%1" == "" goto help 14 | 15 | %SPHINXBUILD% >NUL 2>NUL 16 | if errorlevel 9009 ( 17 | echo. 18 | echo.The 'sphinx-build' command was not found. Make sure you have Sphinx 19 | echo.installed, then set the SPHINXBUILD environment variable to point 20 | echo.to the full path of the 'sphinx-build' executable. Alternatively you 21 | echo.may add the Sphinx directory to PATH. 22 | echo. 23 | echo.If you don't have Sphinx installed, grab it from 24 | echo.http://sphinx-doc.org/ 25 | exit /b 1 26 | ) 27 | 28 | %SPHINXBUILD% -M %1 %SOURCEDIR% %BUILDDIR% %SPHINXOPTS% %O% 29 | goto end 30 | 31 | :help 32 | %SPHINXBUILD% -M help %SOURCEDIR% %BUILDDIR% %SPHINXOPTS% %O% 33 | 34 | :end 35 | popd 36 | -------------------------------------------------------------------------------- /docs/network.rst: -------------------------------------------------------------------------------- 1 | Network Configuration 2 | ===================== 3 | 4 | If you are running franka-interface and frankapy on the same computer, you can skip this page. 5 | 6 | Requirements 7 | ------------ 8 | 9 | * A Control PC computer running Ubuntu 18.04 with a Realtime Kernel. It should also have franka-interface installed with ROS Melodic. 10 | 11 | Terminology 12 | ----------- 13 | 14 | Control PC - Realtime Kernel PC connected by Ethernet to the Robot 15 | 16 | FrankaPy PC - Computer running FrankaPy 17 | 18 | Ethernet 19 | -------- 20 | 21 | 1. Plug an ethernet cable directly between the Control PC and the FrankaPy PC. 22 | 23 | 2. Go into the Ubuntu Network Connections Menu on the Control PC. 24 | 25 | 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. 26 | 27 | 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. 28 | 29 | .. image:: imgs/network_config.png 30 | :width: 600 31 | :alt: Network Config Photo 32 | 33 | 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. 34 | 35 | Wifi 36 | ---- 37 | 38 | While FrankaPy will work over Wifi, it is not recommended due to additional latency in sending commands between computers. 39 | 40 | 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. 41 | 42 | Editing the /etc/hosts file 43 | --------------------------- 44 | 45 | 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. 46 | 47 | 2. On the Control PC, run the command:: 48 | 49 | sudo gedit /etc/hosts 50 | 51 | 3. If you are using an Ethernet connection, then add the following above the line ``# The following lines are desirable for IPv6 capable hosts:`` :: 52 | 53 | 192.168.1.2 [frankapy-pc-name] 54 | 55 | \ 56 | Otherwise substitute ``192.168.1.2`` with the IP address of the FrankaPy PC that you discovered using the command ``ifconfig``. 57 | 58 | 4. Afterwards, on the FrankaPy PC, again run the command ``sudo gedit /etc/hosts`` and add the line:: 59 | 60 | 192.168.1.3 [control-pc-name] 61 | 62 | \ 63 | Otherwise substitute ``192.168.1.3`` with the IP address of the Control PC that you discovered using the command ``ifconfig``. 64 | 65 | 5. Now you should be able to ssh from the FrankaPy PC to the Control PC using the command:: 66 | 67 | ssh [control-pc-username]@[control-pc-name] 68 | Input password to control-pc. 69 | 70 | Setting Up SSH Key to Control PC 71 | -------------------------------- 72 | 73 | Generate a new SSH Key on the FrankaPy PC 74 | ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ 75 | 1. Generate a SSH key by executing the following commands or reading these `instructions `_:: 76 | 77 | ssh-keygen -t rsa -b 4096 -C "your_email@example.com" 78 | [Press enter] 79 | [Press enter] 80 | [Press enter] 81 | eval "$(ssh-agent -s)" 82 | ssh-add ~/.ssh/id_rsa 83 | 84 | Upload the public SSH key to the Control PC 85 | ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ 86 | 87 | 1. In a new terminal on your FrankaPy PC, use your favorite text editor to open your ``id_rsa.pub`` file:: 88 | 89 | gedit ~/.ssh/id_rsa.pub 90 | 91 | 2. Copy the contents in your ``id_rsa.pub`` file. 92 | 93 | 3. Next, SSH to the Control PC:: 94 | 95 | ssh [control-pc-username]@[control-pc-name] 96 | Input password to control-pc. 97 | 98 | 4. Use vim to open the authorized_keys file:: 99 | 100 | vim ~/.ssh/authorized_keys 101 | 102 | 5. Press the following buttons to paste your copied public key into the ``authorized_keys`` file on the Control PC:: 103 | 104 | i 105 | ctrl-shift-v 106 | 107 | : 108 | w 109 | q 110 | 111 | 112 | 6. Open a new terminal on the FrankaPy PC and try SSHing to the Control PC and it should no longer require a password. 113 | 114 | 7. (Optional) Upload your SSH key to Github by following instructions `here `_. -------------------------------------------------------------------------------- /docs/running.rst: -------------------------------------------------------------------------------- 1 | Running the Robot 2 | ================= 3 | 4 | Unlocking the Franka Robot 5 | -------------------------- 6 | 7 | 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``:: 8 | 9 | ssh -X [control-pc-username]@[control-pc-name] 10 | 11 | 2. Open a web browser, either firefox or google chrome using the command line:: 12 | 13 | firefox 14 | 15 | 3. Go to ``172.16.0.2`` in the web browser. 16 | 17 | 4. Login to the Franka Desk GUI using the username and password that you used during the initial robot setup. 18 | 19 | 5. Unlock the robot by clicking the unlock button on the bottom right of the web interface. 20 | 21 | 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. 22 | 23 | 24 | Starting the FrankaPy Interface 25 | ------------------------------- 26 | 27 | 1. Make sure that the Franka Robot has been unlocked in the Franka Desk GUI and has blue lights. 28 | 29 | 2. Open up a new terminal and go to the frankapy directory. 30 | 31 | 3. If you are running franka-interface and frankapy on the same computer, run the following command:: 32 | 33 | bash ./bash_scripts/start_control_pc.sh -i localhost 34 | 35 | 4. Otherwise run the following command:: 36 | 37 | bash ./bash_scripts/start_control_pc.sh -u [control-pc-username] -i [control-pc-name] 38 | 39 | 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. 40 | 41 | 6. Open up a new terminal, enter into the same virtual environment that FrankaPy was installed in, go to the frankapy directory, then:: 42 | 43 | source catkin_ws/devel/setup.bash 44 | 45 | 7. Place your hand on top of the e-stop and reset the robot with the following command:: 46 | 47 | python scripts/reset_arm.py 48 | 49 | 8. See example scripts in the ``examples/`` and ``scripts/`` folders to learn how to use the FrankaPy python package. 50 | 51 | 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``. -------------------------------------------------------------------------------- /docs/support.rst: -------------------------------------------------------------------------------- 1 | Support 2 | ======= 3 | 4 | The easiest way to get help with the library is to join the FrankaPy and Franka-Interface Discord using this link `here `_. 5 | -------------------------------------------------------------------------------- /examples/T_RSD415_franka.tf: -------------------------------------------------------------------------------- 1 | realsense 2 | franka_tool 3 | -0.046490 0.030720 -0.083270 4 | -0.000000 1.000000 0.000000 5 | -1.000000 -0.000000 0.000000 6 | 0.000000 0.000000 1.000000 7 | -------------------------------------------------------------------------------- /examples/T_RS_mount_delta.tf: -------------------------------------------------------------------------------- 1 | franka_tool 2 | franka_tool_base 3 | 0. 0. -0.005 4 | 1. 0. 0. 5 | 0. 1. 0. 6 | 0. 0. 1. -------------------------------------------------------------------------------- /examples/april_tag_pick_place.py: -------------------------------------------------------------------------------- 1 | import os 2 | import logging 3 | import argparse 4 | from time import sleep 5 | 6 | import numpy as np 7 | 8 | from autolab_core import RigidTransform, YamlConfig 9 | from visualization import Visualizer3D as vis3d 10 | 11 | from perception_utils.apriltags import AprilTagDetector 12 | from perception_utils.realsense import get_first_realsense_sensor 13 | 14 | from frankapy import FrankaArm 15 | 16 | 17 | def subsample(data, rate=0.1): 18 | idx = np.random.choice(np.arange(len(data)), size=int(rate * len(data))) 19 | return data[idx] 20 | 21 | 22 | def make_det_one(R): 23 | U, _, Vt = np.linalg.svd(R) 24 | return U @ np.eye(len(R)) @ Vt 25 | 26 | 27 | def get_closest_grasp_pose(T_tag_world, T_ee_world): 28 | tag_axes = [ 29 | T_tag_world.rotation[:,0], -T_tag_world.rotation[:,0], 30 | T_tag_world.rotation[:,1], -T_tag_world.rotation[:,1] 31 | ] 32 | x_axis_ee = T_ee_world.rotation[:,0] 33 | dots = [axis @ x_axis_ee for axis in tag_axes] 34 | grasp_x_axis = tag_axes[np.argmax(dots)] 35 | grasp_z_axis = np.array([0, 0, -1]) 36 | grasp_y_axis = np.cross(grasp_z_axis, grasp_x_axis) 37 | grasp_R = make_det_one(np.c_[grasp_x_axis, grasp_y_axis, grasp_z_axis]) 38 | grasp_translation = T_tag_world.translation + np.array([0, 0, -cfg['cube_size'] / 2]) 39 | return RigidTransform( 40 | rotation=grasp_R, 41 | translation=grasp_translation, 42 | from_frame=T_ee_world.from_frame, to_frame=T_ee_world.to_frame 43 | ) 44 | 45 | 46 | if __name__ == "__main__": 47 | logging.getLogger().setLevel(logging.INFO) 48 | parser = argparse.ArgumentParser() 49 | parser.add_argument('--cfg', '-c', type=str, default='cfg/april_tag_pick_place_cfg.yaml') 50 | parser.add_argument('--no_grasp', '-ng', action='store_true') 51 | args = parser.parse_args() 52 | cfg = YamlConfig(args.cfg) 53 | T_camera_ee = RigidTransform.load(cfg['T_camera_ee_path']) 54 | T_camera_mount_delta = RigidTransform.load(cfg['T_camera_mount_path']) 55 | 56 | logging.info('Starting robot') 57 | fa = FrankaArm() 58 | fa.set_tool_delta_pose(T_camera_mount_delta) 59 | fa.reset_joints() 60 | fa.open_gripper() 61 | 62 | T_ready_world = fa.get_pose() 63 | T_ready_world.translation[0] += 0.25 64 | T_ready_world.translation[2] = 0.4 65 | 66 | fa.goto_pose(T_ready_world) 67 | 68 | logging.info('Init camera') 69 | sensor = get_first_realsense_sensor(cfg['rs']) 70 | sensor.start() 71 | 72 | logging.info('Detecting April Tags') 73 | april = AprilTagDetector(cfg['april_tag']) 74 | intr = sensor.color_intrinsics 75 | T_tag_camera = april.detect(sensor, intr, vis=cfg['vis_detect'])[0] 76 | T_camera_world = T_ready_world * T_camera_ee 77 | T_tag_world = T_camera_world * T_tag_camera 78 | logging.info('Tag has translation {}'.format(T_tag_world.translation)) 79 | 80 | logging.info('Finding closest orthogonal grasp') 81 | T_grasp_world = get_closest_grasp_pose(T_tag_world, T_ready_world) 82 | T_lift = RigidTransform(translation=[0, 0, 0.2], from_frame=T_ready_world.to_frame, to_frame=T_ready_world.to_frame) 83 | T_lift_world = T_lift * T_grasp_world 84 | 85 | logging.info('Visualizing poses') 86 | _, depth_im, _ = sensor.frames() 87 | points_world = T_camera_world * intr.deproject(depth_im) 88 | 89 | if cfg['vis_detect']: 90 | vis3d.figure() 91 | vis3d.pose(RigidTransform()) 92 | vis3d.points(subsample(points_world.data.T), color=(0,1,0), scale=0.002) 93 | vis3d.pose(T_ready_world) 94 | vis3d.pose(T_camera_world) 95 | vis3d.pose(T_tag_world) 96 | vis3d.pose(T_grasp_world) 97 | vis3d.pose(T_lift_world) 98 | vis3d.show() 99 | 100 | if not args.no_grasp: 101 | logging.info('Commanding robot') 102 | fa.goto_pose(T_lift_world, use_impedance=False) 103 | fa.goto_pose(T_grasp_world, use_impedance=False) 104 | fa.close_gripper() 105 | fa.goto_pose(T_lift_world, use_impedance=False) 106 | sleep(3) 107 | fa.goto_pose(T_grasp_world, use_impedance=False) 108 | fa.open_gripper() 109 | fa.goto_pose(T_lift_world, use_impedance=False) 110 | fa.goto_pose(T_ready_world, use_impedance=False) 111 | 112 | import IPython; IPython.embed(); exit(0) 113 | -------------------------------------------------------------------------------- /examples/example_movements.py: -------------------------------------------------------------------------------- 1 | from frankapy import FrankaArm 2 | import numpy as np 3 | from autolab_core import RigidTransform 4 | 5 | if __name__ == '__main__': 6 | 7 | print('Starting robot') 8 | fa = FrankaArm() 9 | 10 | xtranslation_3cm = RigidTransform(rotation=np.array([ 11 | [1, 0, 0], 12 | [0, 1, 0], 13 | [0, 0, 1] 14 | ]), translation=np.array([0.03, 0, 0]), 15 | from_frame='franka_tool', to_frame='world') 16 | 17 | random_position = RigidTransform(rotation=np.array([ 18 | [0.9323473, -0.35858258, 0.04612846], 19 | [-0.35996283, -0.93259467, 0.02597504], 20 | [0.03370496, -0.04082229, -0.99859775] 21 | ]), translation=np.array([0.39247965, -0.21613652, 0.3882055]), 22 | from_frame='franka_tool', to_frame='world') 23 | 24 | print(fa.get_pose().translation) 25 | 26 | print(fa.get_joints()) 27 | 28 | desired_joints_1 = [-0.52733715, 0.25603565, 0.47721503, -1.26705864, 0.00600359, 1.60788199, 0.63019184] 29 | 30 | desired_joints_2 = [-0.16017485, 1.12476619, 0.26004398, -0.67246923, 0.04899213, 2.08439578, 0.81627789] 31 | 32 | fa.reset_joints() 33 | 34 | print('Opening Grippers') 35 | fa.open_gripper() 36 | 37 | #fa.reset_pose() 38 | 39 | # fa.goto_pose(random_position, use_impedance=False, cartesian_impedances=[3000, 3000, 100, 300, 300, 300]) 40 | 41 | fa.goto_joints(desired_joints_1, joint_impedances=[100, 100, 100, 50, 50, 100, 100]) 42 | 43 | fa.goto_joints(desired_joints_2, joint_impedances=[100, 100, 100, 50, 50, 100, 100]) 44 | 45 | #fa.apply_effector_forces_torques(10.0, 0, 0, 0) -------------------------------------------------------------------------------- /examples/execute_joint_torques.py: -------------------------------------------------------------------------------- 1 | import argparse 2 | from frankapy import FrankaArm 3 | 4 | if __name__ == '__main__': 5 | parser = argparse.ArgumentParser() 6 | parser.add_argument('--time', '-t', type=float, default=10) 7 | args = parser.parse_args() 8 | 9 | print('Starting robot') 10 | fa = FrankaArm() 11 | fa.reset_joints() 12 | print('Applying 0 joint torque control for {}s'.format(args.time)) 13 | 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) -------------------------------------------------------------------------------- /examples/go_to_joints_with_joint_impedances.py: -------------------------------------------------------------------------------- 1 | import argparse 2 | import sys 3 | from frankapy import FrankaArm 4 | from frankapy import FrankaConstants as FC 5 | 6 | def wait_for_enter(): 7 | if sys.version_info[0] < 3: 8 | raw_input('Press Enter to continue:') 9 | else: 10 | input('Press Enter to continue:') 11 | 12 | if __name__ == '__main__': 13 | parser = argparse.ArgumentParser() 14 | parser.add_argument('--time', '-t', type=float, default=10) 15 | parser.add_argument('--open_gripper', '-o', action='store_true') 16 | args = parser.parse_args() 17 | 18 | print('Starting robot') 19 | fa = FrankaArm() 20 | if args.open_gripper: 21 | fa.open_gripper() 22 | 23 | print('Be very careful!! Make sure the robot can safely move to HOME JOINTS Position.') 24 | wait_for_enter() 25 | 26 | fa.reset_joints() 27 | print('Using default joint impedances to move back and forth.') 28 | wait_for_enter() 29 | fa.goto_joints(FC.READY_JOINTS, joint_impedances=FC.DEFAULT_JOINT_IMPEDANCES) 30 | fa.goto_joints(FC.HOME_JOINTS) 31 | print('Now using different joint impedances to move back and forth.') 32 | wait_for_enter() 33 | fa.goto_joints(FC.READY_JOINTS, joint_impedances=[1500, 1500, 1500, 1250, 1250, 1000, 1000]) 34 | fa.goto_joints(FC.HOME_JOINTS) 35 | print('Remember to reset the joint_impedances to defaults.') 36 | fa.goto_joints(FC.HOME_JOINTS, joint_impedances=FC.DEFAULT_JOINT_IMPEDANCES) 37 | 38 | -------------------------------------------------------------------------------- /examples/move_robot.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from autolab_core import RigidTransform 3 | 4 | from frankapy import FrankaArm 5 | 6 | 7 | if __name__ == "__main__": 8 | fa = FrankaArm() 9 | 10 | # reset franka to its home joints 11 | fa.reset_joints() 12 | 13 | # read functions 14 | T_ee_world = fa.get_pose() 15 | print('Translation: {} | Rotation: {}'.format(T_ee_world.translation, T_ee_world.quaternion)) 16 | 17 | joints = fa.get_joints() 18 | print('Joints: {}'.format(joints)) 19 | 20 | gripper_width = fa.get_gripper_width() 21 | print('Gripper width: {}'.format(gripper_width)) 22 | 23 | # gripper controls 24 | print('Closing gripper') 25 | fa.close_gripper() 26 | 27 | print('Opening gripper to a specified position') 28 | fa.goto_gripper(0.02) 29 | 30 | print('Opening gripper all the way') 31 | fa.open_gripper() 32 | 33 | # joint controls 34 | print('Rotating last joint') 35 | joints = fa.get_joints() 36 | joints[6] += np.deg2rad(45) 37 | fa.goto_joints(joints) 38 | joints[6] -= np.deg2rad(45) 39 | fa.goto_joints(joints) 40 | 41 | # end-effector pose control 42 | print('Translation') 43 | T_ee_world = fa.get_pose() 44 | T_ee_world.translation += [0.1, 0, 0.1] 45 | fa.goto_pose(T_ee_world) 46 | T_ee_world.translation -= [0.1, 0, 0.1] 47 | fa.goto_pose(T_ee_world) 48 | 49 | print('Rotation in end-effector frame') 50 | T_ee_rot = RigidTransform( 51 | rotation=RigidTransform.x_axis_rotation(np.deg2rad(45)), 52 | from_frame='franka_tool', to_frame='franka_tool' 53 | ) 54 | T_ee_world_target = T_ee_world * T_ee_rot 55 | fa.goto_pose(T_ee_world_target) 56 | fa.goto_pose(T_ee_world) 57 | 58 | # reset franka back to home 59 | fa.reset_joints() -------------------------------------------------------------------------------- /examples/record_trajectory.py: -------------------------------------------------------------------------------- 1 | import argparse 2 | import time 3 | from frankapy import FrankaArm 4 | import pickle as pkl 5 | 6 | if __name__ == '__main__': 7 | parser = argparse.ArgumentParser() 8 | parser.add_argument('--time', '-t', type=float, default=10) 9 | parser.add_argument('--open_gripper', '-o', action='store_true') 10 | parser.add_argument('--file', '-f', default='franka_traj.pkl') 11 | args = parser.parse_args() 12 | 13 | print('Starting robot') 14 | fa = FrankaArm() 15 | if args.open_gripper: 16 | fa.open_gripper() 17 | print('Applying 0 force torque control for {}s'.format(args.time)) 18 | end_effector_position = [] 19 | fa.run_guide_mode(args.time, block=False) 20 | 21 | for i in range(1000): 22 | end_effector_position.append(fa.get_pose()) 23 | time.sleep(0.01) 24 | 25 | pkl.dump(end_effector_position, open(args.file, 'wb')) -------------------------------------------------------------------------------- /examples/run_dynamic_cartesian_velocities.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import time 3 | 4 | from frankapy import FrankaConstants as FC 5 | from frankapy import FrankaArm, SensorDataMessageType 6 | from frankapy.proto_utils import sensor_proto2ros_msg, make_sensor_group_msg 7 | from frankapy.proto import CartesianVelocitySensorMessage, ShouldTerminateSensorMessage 8 | from franka_interface_msgs.msg import SensorDataGroup 9 | 10 | from frankapy.utils import min_jerk 11 | import rospy 12 | 13 | 14 | if __name__ == "__main__": 15 | fa = FrankaArm() 16 | fa.reset_joints() 17 | 18 | rospy.loginfo('Generating Trajectory') 19 | p = fa.get_pose() 20 | p.translation[2] -= 0.2 21 | fa.goto_pose(p, duration=5, block=False) 22 | 23 | cartesian_accelerations = [1, 1, 1, 0.1, 0.1, 0.1] 24 | 25 | T = 5 26 | dt = 0.01 27 | ts = np.arange(0, T, dt) 28 | cartesian_velocities = [] 29 | 30 | for i in range(len(ts)): 31 | cartesian_velocities.append(fa.get_robot_state()['cartesian_velocities']) 32 | time.sleep(dt) 33 | 34 | fa.wait_for_skill() 35 | 36 | print(cartesian_velocities) 37 | 38 | pub = rospy.Publisher(FC.DEFAULT_SENSOR_PUBLISHER_TOPIC, SensorDataGroup, queue_size=1000) 39 | rate = rospy.Rate(1 / dt) 40 | 41 | fa.reset_joints() 42 | 43 | rospy.loginfo('Initializing Sensor Publisher') 44 | 45 | rospy.loginfo('Publishing cartesian velocity trajectory...') 46 | # To ensure skill doesn't end before completing trajectory, make the buffer time much longer than needed 47 | fa.execute_cartesian_velocities(cartesian_velocities[0], cartesian_accelerations, duration=T, dynamic=True, buffer_time=10) 48 | init_time = rospy.Time.now().to_time() 49 | for i in range(len(ts)): 50 | traj_gen_proto_msg = CartesianVelocitySensorMessage( 51 | id=i, timestamp=rospy.Time.now().to_time() - init_time, 52 | cartesian_velocities=cartesian_velocities[i] 53 | ) 54 | ros_msg = make_sensor_group_msg( 55 | trajectory_generator_sensor_msg=sensor_proto2ros_msg( 56 | traj_gen_proto_msg, SensorDataMessageType.CARTESIAN_VELOCITY) 57 | ) 58 | 59 | rospy.loginfo('Publishing: ID {}'.format(traj_gen_proto_msg.id)) 60 | pub.publish(ros_msg) 61 | time.sleep(dt) 62 | 63 | # Stop the skill 64 | # Alternatively can call fa.stop_skill() 65 | term_proto_msg = ShouldTerminateSensorMessage(timestamp=rospy.Time.now().to_time() - init_time, should_terminate=True) 66 | ros_msg = make_sensor_group_msg( 67 | termination_handler_sensor_msg=sensor_proto2ros_msg( 68 | term_proto_msg, SensorDataMessageType.SHOULD_TERMINATE) 69 | ) 70 | pub.publish(ros_msg) 71 | 72 | rospy.loginfo('Done') 73 | -------------------------------------------------------------------------------- /examples/run_dynamic_joint_velocities.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import time 3 | 4 | from frankapy import FrankaConstants as FC 5 | from frankapy import FrankaArm, SensorDataMessageType 6 | from frankapy.proto_utils import sensor_proto2ros_msg, make_sensor_group_msg 7 | from frankapy.proto import JointVelocitySensorMessage, ShouldTerminateSensorMessage 8 | from franka_interface_msgs.msg import SensorDataGroup 9 | 10 | from frankapy.utils import min_jerk 11 | import rospy 12 | 13 | if __name__ == "__main__": 14 | fa = FrankaArm() 15 | fa.reset_joints() 16 | 17 | rospy.loginfo('Generating Trajectory') 18 | p = fa.get_pose() 19 | p.translation[2] -= 0.2 20 | fa.goto_pose(p, duration=5, block=False) 21 | 22 | joint_accelerations = [1, 1, 1, 1, 1, 1, 1] 23 | 24 | T = 5 25 | dt = 0.01 26 | ts = np.arange(0, T, dt) 27 | joint_velocities = [] 28 | 29 | for i in range(len(ts)): 30 | joint_velocities.append(fa.get_robot_state()['joint_velocities']) 31 | time.sleep(dt) 32 | 33 | fa.wait_for_skill() 34 | 35 | pub = rospy.Publisher(FC.DEFAULT_SENSOR_PUBLISHER_TOPIC, SensorDataGroup, queue_size=1000) 36 | rate = rospy.Rate(1 / dt) 37 | 38 | fa.reset_joints() 39 | 40 | rospy.loginfo('Initializing Sensor Publisher') 41 | 42 | rospy.loginfo('Publishing joints trajectory...') 43 | # To ensure skill doesn't end before completing trajectory, make the buffer time much longer than needed 44 | fa.execute_joint_velocities(joint_velocities[0], joint_accelerations, duration=T, dynamic=True, buffer_time=10) 45 | init_time = rospy.Time.now().to_time() 46 | for i in range(len(ts)): 47 | traj_gen_proto_msg = JointVelocitySensorMessage( 48 | id=i, timestamp=rospy.Time.now().to_time() - init_time, 49 | joint_velocities=joint_velocities[i] 50 | ) 51 | ros_msg = make_sensor_group_msg( 52 | trajectory_generator_sensor_msg=sensor_proto2ros_msg( 53 | traj_gen_proto_msg, SensorDataMessageType.JOINT_VELOCITY) 54 | ) 55 | 56 | rospy.loginfo('Publishing: ID {}'.format(traj_gen_proto_msg.id)) 57 | pub.publish(ros_msg) 58 | time.sleep(dt) 59 | 60 | # Stop the skill 61 | # Alternatively can call fa.stop_skill() 62 | term_proto_msg = ShouldTerminateSensorMessage(timestamp=rospy.Time.now().to_time() - init_time, should_terminate=True) 63 | ros_msg = make_sensor_group_msg( 64 | termination_handler_sensor_msg=sensor_proto2ros_msg( 65 | term_proto_msg, SensorDataMessageType.SHOULD_TERMINATE) 66 | ) 67 | pub.publish(ros_msg) 68 | 69 | rospy.loginfo('Done') 70 | -------------------------------------------------------------------------------- /examples/run_dynamic_joints.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | from frankapy import FrankaArm, SensorDataMessageType 4 | from frankapy import FrankaConstants as FC 5 | from frankapy.proto_utils import sensor_proto2ros_msg, make_sensor_group_msg 6 | from frankapy.proto import JointPositionSensorMessage, ShouldTerminateSensorMessage 7 | from franka_interface_msgs.msg import SensorDataGroup 8 | 9 | from frankapy.utils import min_jerk 10 | 11 | import rospy 12 | 13 | 14 | if __name__ == "__main__": 15 | fa = FrankaArm() 16 | fa.reset_joints() 17 | 18 | rospy.loginfo('Generating Trajectory') 19 | joints_0 = fa.get_joints() 20 | p = fa.get_pose() 21 | p.translation[2] -= 0.2 22 | fa.goto_pose(p) 23 | joints_1 = fa.get_joints() 24 | 25 | T = 5 26 | dt = 0.02 27 | ts = np.arange(0, T, dt) 28 | joints_traj = [min_jerk(joints_1, joints_0, t, T) for t in ts] 29 | 30 | rospy.loginfo('Initializing Sensor Publisher') 31 | pub = rospy.Publisher(FC.DEFAULT_SENSOR_PUBLISHER_TOPIC, SensorDataGroup, queue_size=1000) 32 | rate = rospy.Rate(1 / dt) 33 | 34 | rospy.loginfo('Publishing joints trajectory...') 35 | # To ensure skill doesn't end before completing trajectory, make the buffer time much longer than needed 36 | fa.goto_joints(joints_traj[1], duration=T, dynamic=True, buffer_time=10) 37 | init_time = rospy.Time.now().to_time() 38 | for i in range(2, len(ts)): 39 | traj_gen_proto_msg = JointPositionSensorMessage( 40 | id=i, timestamp=rospy.Time.now().to_time() - init_time, 41 | joints=joints_traj[i] 42 | ) 43 | ros_msg = make_sensor_group_msg( 44 | trajectory_generator_sensor_msg=sensor_proto2ros_msg( 45 | traj_gen_proto_msg, SensorDataMessageType.JOINT_POSITION) 46 | ) 47 | 48 | rospy.loginfo('Publishing: ID {}'.format(traj_gen_proto_msg.id)) 49 | pub.publish(ros_msg) 50 | rate.sleep() 51 | 52 | # Stop the skill 53 | # Alternatively can call fa.stop_skill() 54 | term_proto_msg = ShouldTerminateSensorMessage(timestamp=rospy.Time.now().to_time() - init_time, should_terminate=True) 55 | ros_msg = make_sensor_group_msg( 56 | termination_handler_sensor_msg=sensor_proto2ros_msg( 57 | term_proto_msg, SensorDataMessageType.SHOULD_TERMINATE) 58 | ) 59 | pub.publish(ros_msg) 60 | 61 | rospy.loginfo('Done') 62 | -------------------------------------------------------------------------------- /examples/run_dynamic_pose.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | from autolab_core import RigidTransform 4 | from frankapy import FrankaArm, SensorDataMessageType 5 | from frankapy import FrankaConstants as FC 6 | from frankapy.proto_utils import sensor_proto2ros_msg, make_sensor_group_msg 7 | from frankapy.proto import PosePositionSensorMessage, ShouldTerminateSensorMessage, CartesianImpedanceSensorMessage 8 | from franka_interface_msgs.msg import SensorDataGroup 9 | 10 | from frankapy.utils import min_jerk, min_jerk_weight 11 | 12 | import rospy 13 | 14 | 15 | if __name__ == "__main__": 16 | fa = FrankaArm() 17 | fa.reset_joints() 18 | 19 | rospy.loginfo('Generating Trajectory') 20 | p0 = fa.get_pose() 21 | p1 = p0.copy() 22 | T_delta = RigidTransform( 23 | translation=np.array([0, 0, 0.2]), 24 | rotation=RigidTransform.z_axis_rotation(np.deg2rad(30)), 25 | from_frame=p1.from_frame, to_frame=p1.from_frame) 26 | p1 = p1 * T_delta 27 | fa.goto_pose(p1) 28 | 29 | T = 5 30 | dt = 0.02 31 | ts = np.arange(0, T, dt) 32 | has_closed = False 33 | 34 | weights = [min_jerk_weight(t, T) for t in ts] 35 | pose_traj = [p1.interpolate_with(p0, w) for w in weights] 36 | 37 | z_stiffness_traj = [min_jerk(100, 800, t, T) for t in ts] 38 | 39 | rospy.loginfo('Initializing Sensor Publisher') 40 | pub = rospy.Publisher(FC.DEFAULT_SENSOR_PUBLISHER_TOPIC, SensorDataGroup, queue_size=1000) 41 | rate = rospy.Rate(1 / dt) 42 | 43 | rospy.loginfo('Publishing pose trajectory...') 44 | # To ensure skill doesn't end before completing trajectory, make the buffer time much longer than needed 45 | fa.goto_pose(pose_traj[1], duration=T, dynamic=True, buffer_time=10, 46 | cartesian_impedances=FC.DEFAULT_TRANSLATIONAL_STIFFNESSES[:2] + [z_stiffness_traj[1]] + FC.DEFAULT_ROTATIONAL_STIFFNESSES 47 | ) 48 | init_time = rospy.Time.now().to_time() 49 | for i in range(2, len(ts)): 50 | timestamp = rospy.Time.now().to_time() - init_time 51 | traj_gen_proto_msg = PosePositionSensorMessage( 52 | id=i, timestamp=timestamp, 53 | position=pose_traj[i].translation, quaternion=pose_traj[i].quaternion 54 | ) 55 | fb_ctrlr_proto = CartesianImpedanceSensorMessage( 56 | id=i, timestamp=timestamp, 57 | translational_stiffnesses=FC.DEFAULT_TRANSLATIONAL_STIFFNESSES[:2] + [z_stiffness_traj[i]], 58 | rotational_stiffnesses=FC.DEFAULT_ROTATIONAL_STIFFNESSES 59 | ) 60 | ros_msg = make_sensor_group_msg( 61 | trajectory_generator_sensor_msg=sensor_proto2ros_msg( 62 | traj_gen_proto_msg, SensorDataMessageType.POSE_POSITION), 63 | feedback_controller_sensor_msg=sensor_proto2ros_msg( 64 | fb_ctrlr_proto, SensorDataMessageType.CARTESIAN_IMPEDANCE) 65 | ) 66 | 67 | rospy.loginfo('Publishing: ID {}'.format(traj_gen_proto_msg.id)) 68 | pub.publish(ros_msg) 69 | rate.sleep() 70 | 71 | # Stop the skill 72 | # Alternatively can call fa.stop_skill() 73 | term_proto_msg = ShouldTerminateSensorMessage(timestamp=rospy.Time.now().to_time() - init_time, should_terminate=True) 74 | ros_msg = make_sensor_group_msg( 75 | termination_handler_sensor_msg=sensor_proto2ros_msg( 76 | term_proto_msg, SensorDataMessageType.SHOULD_TERMINATE) 77 | ) 78 | pub.publish(ros_msg) 79 | 80 | rospy.loginfo('Done') 81 | -------------------------------------------------------------------------------- /examples/run_hfpc.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | from frankapy import FrankaArm, SensorDataMessageType 4 | from frankapy import FrankaConstants as FC 5 | from frankapy.proto_utils import sensor_proto2ros_msg, make_sensor_group_msg 6 | from frankapy.proto import ForcePositionSensorMessage, ForcePositionControllerSensorMessage 7 | from franka_interface_msgs.msg import SensorDataGroup 8 | from frankapy.utils import transform_to_list, min_jerk 9 | 10 | from tqdm import trange 11 | 12 | import rospy 13 | 14 | 15 | if __name__ == "__main__": 16 | fa = FrankaArm() 17 | fa.reset_joints() 18 | fa.close_gripper() 19 | 20 | while True: 21 | input('Presse [Enter] to enter guide mode and move robot to be on top of a flat surface.') 22 | fa.run_guide_mode() 23 | while True: 24 | inp = input('[r]etry or [c]ontinue? ') 25 | if inp not in ('r', 'c'): 26 | print('Please give valid input!') 27 | else: 28 | break 29 | if inp == 'c': 30 | break 31 | 32 | rospy.loginfo('Generating Trajectory') 33 | # EE will follow a 2D circle while pressing down with a target force 34 | dt = 0.01 35 | T = 10 36 | ts = np.arange(0, T, dt) 37 | N = len(ts) 38 | dthetas = np.linspace(-np.pi / 2, 3 * np.pi / 2, N) 39 | r = 0.07 40 | circ = r * np.c_[np.sin(dthetas), np.cos(dthetas)] 41 | 42 | start_pose = fa.get_pose() 43 | start_pose.rotation = FC.HOME_POSE.rotation 44 | target_poses = [] 45 | for i, t in enumerate(ts): 46 | pose = start_pose.copy() 47 | pose.translation[0] += r + circ[i, 0] 48 | pose.translation[1] += circ[i, 1] 49 | target_poses.append(pose) 50 | target_force = [0, 0, -10, 0, 0, 0] 51 | S = [1, 1, 1, 1, 1, 1] 52 | position_kps_cart = FC.DEFAULT_TRANSLATIONAL_STIFFNESSES + FC.DEFAULT_ROTATIONAL_STIFFNESSES 53 | force_kps_cart = [0.1] * 6 54 | position_kps_joint = FC.DEFAULT_K_GAINS 55 | force_kps_joint = [0.1] * 7 56 | 57 | rospy.loginfo('Initializing Sensor Publisher') 58 | pub = rospy.Publisher(FC.DEFAULT_SENSOR_PUBLISHER_TOPIC, SensorDataGroup, queue_size=1) 59 | rate = rospy.Rate(1 / dt) 60 | n_times = 1 61 | 62 | rospy.loginfo('Publishing HFPC trajectory w/ cartesian gains...') 63 | fa.run_dynamic_force_position(duration=T * n_times, buffer_time=3, S=S, 64 | use_cartesian_gains=True, 65 | position_kps_cart=position_kps_cart, 66 | force_kps_cart=force_kps_cart) 67 | init_time = rospy.Time.now().to_time() 68 | for i in trange(N * n_times): 69 | t = i % N 70 | timestamp = rospy.Time.now().to_time() - init_time 71 | traj_gen_proto_msg = ForcePositionSensorMessage( 72 | id=i, timestamp=timestamp, seg_run_time=dt, 73 | pose=transform_to_list(target_poses[t]), 74 | force=target_force 75 | ) 76 | fb_ctrlr_proto = ForcePositionControllerSensorMessage( 77 | id=i, timestamp=timestamp, 78 | position_kps_cart=position_kps_cart, 79 | force_kps_cart=force_kps_cart, 80 | selection=S 81 | ) 82 | ros_msg = make_sensor_group_msg( 83 | trajectory_generator_sensor_msg=sensor_proto2ros_msg( 84 | traj_gen_proto_msg, SensorDataMessageType.FORCE_POSITION), 85 | feedback_controller_sensor_msg=sensor_proto2ros_msg( 86 | fb_ctrlr_proto, SensorDataMessageType.FORCE_POSITION_GAINS) 87 | ) 88 | pub.publish(ros_msg) 89 | rate.sleep() 90 | fa.stop_skill() 91 | 92 | rospy.loginfo('Publishing HFPC trajectory w/ joint gains...') 93 | fa.run_dynamic_force_position(duration=T * n_times, buffer_time=3, S=S, 94 | use_cartesian_gains=False, 95 | position_kps_joint=position_kps_joint, 96 | force_kps_joint=force_kps_joint) 97 | init_time = rospy.Time.now().to_time() 98 | for i in trange(N * n_times): 99 | t = i % N 100 | timestamp = rospy.Time.now().to_time() - init_time 101 | traj_gen_proto_msg = ForcePositionSensorMessage( 102 | id=i, timestamp=timestamp, seg_run_time=dt, 103 | pose=transform_to_list(target_poses[t]), 104 | force=target_force 105 | ) 106 | fb_ctrlr_proto = ForcePositionControllerSensorMessage( 107 | id=i, timestamp=timestamp, 108 | position_kps_joint=position_kps_joint, 109 | force_kps_joint=force_kps_joint, 110 | selection=S 111 | ) 112 | ros_msg = make_sensor_group_msg( 113 | trajectory_generator_sensor_msg=sensor_proto2ros_msg( 114 | traj_gen_proto_msg, SensorDataMessageType.FORCE_POSITION), 115 | feedback_controller_sensor_msg=sensor_proto2ros_msg( 116 | fb_ctrlr_proto, SensorDataMessageType.FORCE_POSITION_GAINS) 117 | ) 118 | pub.publish(ros_msg) 119 | rate.sleep() 120 | fa.stop_skill() 121 | 122 | fa.reset_joints() 123 | fa.open_gripper() 124 | 125 | rospy.loginfo('Done') 126 | -------------------------------------------------------------------------------- /examples/run_pose_dmp.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import math 3 | import rospy 4 | import argparse 5 | import pickle 6 | from autolab_core import RigidTransform, Point 7 | from frankapy import FrankaArm 8 | 9 | if __name__ == '__main__': 10 | parser = argparse.ArgumentParser() 11 | parser.add_argument('--pose_dmp_weights_file_path', '-f', type=str) 12 | args = parser.parse_args() 13 | 14 | print('Starting robot') 15 | fa = FrankaArm(); 16 | 17 | with open(args.pose_dmp_weights_file_path, 'rb') as pkl_f: 18 | pose_dmp_info = pickle.load(pkl_f) 19 | fa.execute_pose_dmp(pose_dmp_info, duration=8) -------------------------------------------------------------------------------- /examples/run_quaternion_pose_dmp.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import math 3 | import rospy 4 | import argparse 5 | import pickle 6 | from autolab_core import RigidTransform, Point 7 | from frankapy import FrankaArm 8 | 9 | def execute_quaternion_pose_dmp(fa, position_dmp_weights_path, quat_dmp_weights_path, 10 | goal_quat=(0.03, 1.0, -0.03, 0.01), 11 | duration=10.0): 12 | position_dmp_file = open(position_dmp_weights_path, 'rb') 13 | position_dmp_info = pickle.load(position_dmp_file) 14 | 15 | quat_dmp_file = open(quat_dmp_weights_path, 'rb') 16 | quat_dmp_info = pickle.load(quat_dmp_file) 17 | 18 | # Should be less than duration so that the canonical system is set to 0 appropriately 19 | quat_goal_time = duration - 3.0 20 | fa.execute_quaternion_pose_dmp(position_dmp_info, quat_dmp_info, duration, goal_quat, quat_goal_time) 21 | 22 | if __name__ == '__main__': 23 | parser = argparse.ArgumentParser() 24 | parser.add_argument('--position_dmp_weights_path', '-p', type=str) 25 | parser.add_argument('--quat_dmp_weights_path', '-q', type=str) 26 | args = parser.parse_args() 27 | 28 | print('Starting robot') 29 | fa = FrankaArm(); 30 | 31 | execute_quaternion_pose_dmp(fa, 32 | args.position_dmp_weights_path, 33 | args.quat_dmp_weights_path, 34 | goal_quat=(0.03, 1.0, -0.03, 0.01), 35 | duration=20.0): 36 | -------------------------------------------------------------------------------- /examples/run_recorded_trajectory.py: -------------------------------------------------------------------------------- 1 | import pickle as pkl 2 | import numpy as np 3 | 4 | from autolab_core import RigidTransform 5 | from frankapy import FrankaArm, SensorDataMessageType 6 | from frankapy import FrankaConstants as FC 7 | from frankapy.proto_utils import sensor_proto2ros_msg, make_sensor_group_msg 8 | from frankapy.proto import PosePositionSensorMessage, ShouldTerminateSensorMessage, CartesianImpedanceSensorMessage 9 | from franka_interface_msgs.msg import SensorDataGroup 10 | 11 | from frankapy.utils import min_jerk, min_jerk_weight 12 | 13 | import rospy 14 | 15 | if __name__ == "__main__": 16 | fa = FrankaArm() 17 | fa.reset_joints() 18 | 19 | rospy.loginfo('Generating Trajectory') 20 | 21 | pose_traj = pkl.load(open('franka_traj.pkl','rb')) 22 | 23 | T = 10 24 | dt = 0.01 25 | ts = np.arange(0, T, dt) 26 | 27 | rospy.loginfo('Initializing Sensor Publisher') 28 | pub = rospy.Publisher(FC.DEFAULT_SENSOR_PUBLISHER_TOPIC, SensorDataGroup, queue_size=10) 29 | rate = rospy.Rate(1 / dt) 30 | 31 | rospy.loginfo('Publishing pose trajectory...') 32 | # To ensure skill doesn't end before completing trajectory, make the buffer time much longer than needed 33 | fa.goto_pose(pose_traj[1], duration=T, dynamic=True, buffer_time=10, 34 | cartesian_impedances=[600.0, 600.0, 600.0, 50.0, 50.0, 50.0] 35 | ) 36 | init_time = rospy.Time.now().to_time() 37 | for i in range(2, len(ts)): 38 | timestamp = rospy.Time.now().to_time() - init_time 39 | traj_gen_proto_msg = PosePositionSensorMessage( 40 | id=i, timestamp=timestamp, 41 | position=pose_traj[i].translation, quaternion=pose_traj[i].quaternion 42 | ) 43 | ros_msg = make_sensor_group_msg( 44 | trajectory_generator_sensor_msg=sensor_proto2ros_msg( 45 | traj_gen_proto_msg, SensorDataMessageType.POSE_POSITION), 46 | ) 47 | 48 | rospy.loginfo('Publishing: ID {}'.format(traj_gen_proto_msg.id)) 49 | pub.publish(ros_msg) 50 | rate.sleep() 51 | 52 | # Stop the skill 53 | # Alternatively can call fa.stop_skill() 54 | term_proto_msg = ShouldTerminateSensorMessage(timestamp=rospy.Time.now().to_time() - init_time, should_terminate=True) 55 | ros_msg = make_sensor_group_msg( 56 | termination_handler_sensor_msg=sensor_proto2ros_msg( 57 | term_proto_msg, SensorDataMessageType.SHOULD_TERMINATE) 58 | ) 59 | pub.publish(ros_msg) 60 | 61 | rospy.loginfo('Done') 62 | -------------------------------------------------------------------------------- /frankapy/__init__.py: -------------------------------------------------------------------------------- 1 | from .franka_arm import FrankaArm 2 | from .franka_constants import FrankaConstants 3 | from .franka_arm_state_client import FrankaArmStateClient 4 | from .exceptions import FrankaArmCommException 5 | from .franka_interface_common_definitions import SkillType, MetaSkillType, TrajectoryGeneratorType, FeedbackControllerType, TerminationHandlerType, SkillStatus, SensorDataMessageType -------------------------------------------------------------------------------- /frankapy/exceptions.py: -------------------------------------------------------------------------------- 1 | class FrankaArmCommException(Exception): 2 | ''' Communication failure. Usually occurs due to timeouts. 3 | ''' 4 | def __init__(self, message, *args, **kwargs): 5 | Exception.__init__(self, *args, **kwargs) 6 | self.message = message 7 | 8 | def __str__(self): 9 | return "Communication w/ FrankaInterface ran into a problem: {}. FrankaInterface is probably not ready.".format(self.message) 10 | 11 | 12 | class FrankaArmFrankaInterfaceNotReadyException(Exception): 13 | ''' Exception for when franka_interface is not ready 14 | ''' 15 | 16 | def __init__(self, *args, **kwargs): 17 | Exception.__init__(self, *args, **kwargs) 18 | 19 | def __str__(self): 20 | return 'FrankaInterface was not ready!' 21 | 22 | 23 | class FrankaArmException(Exception): 24 | ''' Failure of control, typically due to a kinematically unreachable pose. 25 | ''' 26 | 27 | def __init__(self, message, *args, **kwargs): 28 | Exception.__init__(self, *args, **kwargs) 29 | self.message = message 30 | 31 | def __str__(self): 32 | return "FrankaInterface ran into a problem: {}".format(self.message) 33 | -------------------------------------------------------------------------------- /frankapy/franka_arm_state_client.py: -------------------------------------------------------------------------------- 1 | import logging 2 | 3 | import numpy as np 4 | import rospy 5 | from franka_interface_msgs.srv import GetCurrentRobotStateCmd 6 | 7 | from .utils import franka_pose_to_rigid_transform 8 | 9 | 10 | class FrankaArmStateClient: 11 | 12 | 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): 13 | if new_ros_node: 14 | rospy.init_node('FrankaArmStateClient', anonymous=True) 15 | 16 | self._offline = offline 17 | if not self._offline: 18 | rospy.wait_for_service(robot_state_server_name) 19 | self._get_current_robot_state = rospy.ServiceProxy(robot_state_server_name, GetCurrentRobotStateCmd) 20 | 21 | def get_data(self): 22 | '''Get all fields of current robot data in a dict. 23 | 24 | Returns: 25 | dict of robot state 26 | ''' 27 | if self._offline: 28 | logging.warn('In offline mode - FrankaArmStateClient will return 0 values.') 29 | return { 30 | 'cartesian_velocities': np.zeros(6), 31 | 'pose': franka_pose_to_rigid_transform(np.eye(4)), 32 | 'pose_desired': franka_pose_to_rigid_transform(np.eye(4)), 33 | 'joint_torques': np.zeros(7), 34 | 'joint_torques_derivative': np.zeros(7), 35 | 'joints': np.zeros(7), 36 | 'joints_desired': np.zeros(7), 37 | 'joint_velocities': np.zeros(7), 38 | 'gripper_width': 0, 39 | 'gripper_is_grasped': False, 40 | 'ee_force_torque': np.zeros(6) 41 | } 42 | 43 | ros_data = self._get_current_robot_state().robot_state 44 | 45 | data = { 46 | 'cartesian_velocities': np.array(ros_data.O_dP_EE_c), 47 | 'pose': franka_pose_to_rigid_transform(ros_data.O_T_EE), 48 | 'pose_desired': franka_pose_to_rigid_transform(ros_data.O_T_EE_d), 49 | 'joint_torques': np.array(ros_data.tau_J), 50 | 'joint_torques_derivative': np.array(ros_data.dtau_J), 51 | 'joints': np.array(ros_data.q), 52 | 'joints_desired': np.array(ros_data.q_d), 53 | 'joint_velocities': np.array(ros_data.dq), 54 | 'gripper_width': ros_data.gripper_width, 55 | 'gripper_is_grasped': ros_data.gripper_is_grasped, 56 | 'ee_force_torque': np.array(ros_data.O_F_ext_hat_K) 57 | } 58 | 59 | return data 60 | 61 | def get_pose(self): 62 | '''Get the current pose. 63 | 64 | Returns: 65 | RigidTransform 66 | ''' 67 | return self.get_data()['pose'] 68 | 69 | def get_joints(self): 70 | '''Get the current joint configuration. 71 | 72 | Returns: 73 | ndarray of shape (7,) 74 | ''' 75 | return self.get_data()['joints'] 76 | 77 | def get_joint_torques(self): 78 | '''Get the current joint torques. 79 | 80 | Returns: 81 | ndarray of shape (7,) 82 | ''' 83 | return self.get_data()['joint_torques'] 84 | 85 | def get_joint_velocities(self): 86 | '''Get the current joint velocities. 87 | 88 | Returns: 89 | ndarray of shape (7,) 90 | ''' 91 | return self.get_data()['joint_velocities'] 92 | 93 | def get_gripper_width(self): 94 | '''Get most recent gripper width. Note this value will *not* be updated 95 | during a control command. 96 | 97 | Returns: 98 | float of gripper width in meters 99 | ''' 100 | return self.get_data()['gripper_width'] 101 | 102 | def get_gripper_is_grasped(self): 103 | '''Returns whether or not the gripper is grasping something. Note this 104 | value will *not* be updated during a control command. 105 | 106 | Returns: 107 | True if gripper is grasping something. False otherwise 108 | ''' 109 | return self.get_data()['gripper_is_grasped'] 110 | 111 | def get_ee_force_torque(self): 112 | '''Get the current ee force torque in base frame. 113 | 114 | Returns: 115 | ndarray of shape (6,) 116 | ''' 117 | 118 | return self.get_data()['ee_force_torque'] -------------------------------------------------------------------------------- /frankapy/franka_constants.py: -------------------------------------------------------------------------------- 1 | import logging 2 | import math 3 | import numpy as np 4 | from autolab_core import RigidTransform 5 | 6 | class FrankaConstants: 7 | ''' 8 | Contains default robot values, as well as robot limits. 9 | All units are in SI. 10 | ''' 11 | 12 | LOGGING_LEVEL = logging.INFO 13 | 14 | EMPTY_SENSOR_VALUES = [0] 15 | 16 | # translational stiffness, rotational stiffness 17 | DEFAULT_FORCE_AXIS_TRANSLATIONAL_STIFFNESS = 600 18 | DEFAULT_FORCE_AXIS_ROTATIONAL_STIFFNESS = 20 19 | 20 | # buffer time 21 | DEFAULT_TERM_BUFFER_TIME = 0.2 22 | 23 | HOME_JOINTS = [0, -math.pi / 4, 0, -3 * math.pi / 4, 0, math.pi / 2, math.pi / 4] 24 | HOME_POSE = RigidTransform(rotation=np.array([ 25 | [1, 0, 0], 26 | [0, -1, 0], 27 | [0, 0, -1], 28 | ]), translation=np.array([0.3069, 0, 0.4867]), 29 | from_frame='franka_tool', to_frame='world') 30 | READY_JOINTS = [0, -math.pi/4, 0, -2.85496998, 0, 2.09382820, math.pi/4] 31 | READY_POSE = RigidTransform(rotation=np.array([ 32 | [1, 0, 0], 33 | [0, -1, 0], 34 | [0, 0, -1], 35 | ]), translation=np.array([0.3069, 0, 0.2867]), 36 | from_frame='franka_tool', to_frame='world') 37 | 38 | # See https://frankaemika.github.io/docs/control_parameters.html 39 | JOINT_LIMITS_MIN = [-2.8973, -1.7628, -2.8973, -3.0718, -2.8973, -0.0175, -2.8973] 40 | JOINT_LIMITS_MAX = [2.8973, 1.7628, 2.8973, -0.0698, 2.8973, 3.7525, 2.8973] 41 | 42 | DEFAULT_POSE_THRESHOLDS = [0.0001, 0.0001, 0.0001, 0.0001, 0.0001, 0.0001] 43 | DEFAULT_JOINT_THRESHOLDS = [0.0001, 0.0001, 0.0001, 0.0001, 0.0001, 0.0001, 0.0001] 44 | 45 | GRIPPER_WIDTH_MAX = 0.08 46 | GRIPPER_WIDTH_MIN = 0 47 | GRIPPER_MAX_FORCE = 60 48 | 49 | MAX_LIN_MOMENTUM = 20 50 | MAX_ANG_MOMENTUM = 2 51 | MAX_LIN_MOMENTUM_CONSTRAINED = 100 52 | 53 | DEFAULT_FRANKA_INTERFACE_TIMEOUT = 10 54 | ACTION_WAIT_LOOP_TIME = 0.001 55 | 56 | GRIPPER_CMD_SLEEP_TIME = 0.2 57 | 58 | DEFAULT_K_GAINS = [600.0, 600.0, 600.0, 600.0, 250.0, 150.0, 50.0] 59 | DEFAULT_D_GAINS = [50.0, 50.0, 50.0, 50.0, 30.0, 25.0, 15.0] 60 | DEFAULT_TRANSLATIONAL_STIFFNESSES = [600.0, 600.0, 600.0] 61 | DEFAULT_ROTATIONAL_STIFFNESSES = [50.0, 50.0, 50.0] 62 | 63 | DEFAULT_JOINT_IMPEDANCES = [3000, 3000, 3000, 2500, 2500, 2000, 2000] 64 | DEFAULT_CARTESIAN_IMPEDANCES = [3000, 3000, 3000, 300, 300, 300] 65 | 66 | DEFAULT_LOWER_TORQUE_THRESHOLDS_ACCEL = [20.0,20.0,18.0,18.0,16.0,14.0,12.0] 67 | DEFAULT_UPPER_TORQUE_THRESHOLDS_ACCEL = [120.0,120.0,120.0,118.0,116.0,114.0,112.0] 68 | DEFAULT_LOWER_TORQUE_THRESHOLDS_NOMINAL = [20.0,20.0,18.0,18.0,16.0,14.0,12.0] 69 | DEFAULT_UPPER_TORQUE_THRESHOLDS_NOMINAL = [120.0,120.0,118.0,118.0,116.0,114.0,112.0] 70 | 71 | DEFAULT_LOWER_FORCE_THRESHOLDS_ACCEL = [10.0,10.0,10.0,10.0,10.0,10.0] 72 | DEFAULT_UPPER_FORCE_THRESHOLDS_ACCEL = [120.0,120.0,120.0,125.0,125.0,125.0] 73 | DEFAULT_LOWER_FORCE_THRESHOLDS_NOMINAL = [10.0,10.0,10.0,10.0,10.0,10.0] 74 | DEFAULT_UPPER_FORCE_THRESHOLDS_NOMINAL = [120.0,120.0,120.0,125.0,125.0,125.0] 75 | 76 | DH_PARAMS = np.array([[0, 0.333, 0, 0], 77 | [0, 0, -np.pi/2, 0], 78 | [0, 0.316, np.pi/2, 0], 79 | [0.0825, 0, np.pi/2, 0], 80 | [-0.0825, 0.384, -np.pi/2, 0], 81 | [0, 0, np.pi/2, 0], 82 | [0.088, 0, np.pi/2, 0], 83 | [0, 0.107, 0, 0], 84 | [0, 0.1034, 0, 0]]) 85 | 86 | N_REV_JOINTS = 7 87 | 88 | COLLISION_BOX_SHAPES = np.array([ 89 | [0.23, 0.2, 0.1], 90 | [0.13, 0.12, 0.1], 91 | [0.12, 0.1, 0.2], 92 | [0.15, 0.27, 0.11], 93 | [0.12, 0.1, 0.2], 94 | [0.13, 0.12, 0.25], 95 | [0.13, 0.23, 0.15], 96 | [0.12, 0.12, 0.4], 97 | [0.12, 0.12, 0.25], 98 | [0.13, 0.23, 0.12], 99 | [0.12, 0.12, 0.2], 100 | [0.08, 0.22, 0.17] 101 | ]) 102 | 103 | COLLISION_BOX_LINKS = [1, 1, 1, 1, 1, 3, 4, 5, 5, 5, 7, 7] 104 | 105 | COLLISION_BOX_POSES = np.array([ 106 | [[ 1. , 0. , 0. , -0.04 ], 107 | [ 0. , 1. , 0. , 0. ], 108 | [ 0. , 0. , 1. , -0.283 ], 109 | [ 0. , 0. , 0. , 1. ]], 110 | [[ 1. , 0. , 0. , -0.009 ], 111 | [ 0. , 1. , 0. , 0. ], 112 | [ 0. , 0. , 1. , -0.183 ], 113 | [ 0. , 0. , 0. , 1. ]], 114 | [[ 1. , 0. , 0. , 0. ], 115 | [ 0. , 0.81038486, -0.58589793, -0.032 ], 116 | [ 0. , 0.58589793, 0.81038486, -0.082 ], 117 | [ 0. , 0. , 0. , 1. ]], 118 | [[ 1. , 0. , 0. , -0.008 ], 119 | [ 0. , 1. , 0. , 0. ], 120 | [ 0. , 0. , 1. , 0. ], 121 | [ 0. , 0. , 0. , 1. ]], 122 | [[ 1. , 0. , 0. , 0. ], 123 | [ 0. , 0.81038486, -0.58589793, 0.042 ], 124 | [ 0. , 0.58589793, 0.81038486, 0.067 ], 125 | [ 0. , 0. , 0. , 1. ]], 126 | [[ 1. , 0. , 0. , 0.00687 ], 127 | [ 0. , 1. , 0. , 0. ], 128 | [ 0. , 0. , 1. , -0.139 ], 129 | [ 0. , 0. , 0. , 1. ]], 130 | [[ 1. , -0. , 0. , -0.008 ], 131 | [ 0. , 0. , 1. , 0.004 ], 132 | [-0. , -1. , 0. , 0. ], 133 | [ 0. , 0. , 0. , 1. ]], 134 | [[ 1. , -0. , 0. , 0.00422 ], 135 | [ 0. , 0.98480775, 0.17364817, 0.05367 ], 136 | [-0. , -0.17364817, 0.98480775, -0.121 ], 137 | [ 0. , 0. , 0. , 1. ]], 138 | [[ 1. , 0. , 0. , 0.00422 ], 139 | [ 0. , 1. , 0. , 0.00367 ], 140 | [ 0. , 0. , 1. , -0.263 ], 141 | [ 0. , 0. , 0. , 1. ]], 142 | [[ 1. , 0. , 0. , 0.00328 ], 143 | [ 0. , 1. , 0. , 0.0176 ], 144 | [ 0. , 0. , 1. , -0.0055 ], 145 | [ 0. , 0. , 0. , 1. ]], 146 | [[ 1. , 0. , 0. , -0.0136 ], 147 | [ 0. , -1. , 0. , 0.0092 ], 148 | [ 0. , 0. , -1. , 0.0083 ], 149 | [ 0. , 0. , 0. , 1. ]], 150 | [[ 0.70710678, 0.70710678, 0. , 0.0136 ], 151 | [-0.70710678, 0.70710678, -0. , -0.0092 ], 152 | [-0. , 0. , 1. , 0.1457 ], 153 | [ 0. , 0. , 0. , 1. ]]] 154 | ) 155 | 156 | JOINT_NAMES = ['panda_joint1', 157 | 'panda_joint2', 158 | 'panda_joint3', 159 | 'panda_joint4', 160 | 'panda_joint5', 161 | 'panda_joint6', 162 | 'panda_joint7', 163 | 'panda_finger_joint1', 164 | 'panda_finger_joint2'] 165 | 166 | WORKSPACE_WALLS = np.array([ 167 | # sides 168 | [0.15, 0.46, 0.5, 0, 0, 0, 1.2, 0.01, 1.1], 169 | [0.15, -0.46, 0.5, 0, 0, 0, 1.2, 0.01, 1.1], 170 | # back 171 | [-0.41, 0, 0.5, 0, 0, 0, 0.01, 1, 1.1], 172 | # front 173 | [0.75, 0, 0.5, 0, 0, 0, 0.01, 1, 1.1], 174 | # top 175 | [0.2, 0, 1, 0, 0, 0, 1.2, 1, 0.01], 176 | # bottom 177 | [0.2, 0, -0.05, 0, 0, 0, 1.2, 1, 0.01] 178 | ]) 179 | 180 | DEFAULT_SENSOR_PUBLISHER_TOPIC = 'franka_ros_interface/sensor' 181 | DYNAMIC_SKILL_WAIT_TIME = 0.3 182 | 183 | DEFAULT_HFPC_FORCE_GAIN = [0.1] * 6 184 | DEFAULT_HFPC_S = [1, 1, 1, 1, 1, 1] -------------------------------------------------------------------------------- /frankapy/franka_interface_common_definitions.py: -------------------------------------------------------------------------------- 1 | ####################################################################### 2 | # # 3 | # Important: Any Changes here should also be reflected in changes # 4 | # in the franka-interface-common definitions.h file as well. # 5 | # # 6 | # The order of the enums matter!! # 7 | # # 8 | ####################################################################### 9 | 10 | 11 | _ENUM_COUNTER = {} 12 | def _enum_auto(key): 13 | if key not in _ENUM_COUNTER: 14 | _ENUM_COUNTER[key] = 0 15 | val = _ENUM_COUNTER[key] 16 | _ENUM_COUNTER[key] += 1 17 | return val 18 | 19 | 20 | class SkillType: 21 | CartesianPoseSkill = _enum_auto('SkillType') 22 | CartesianVelocitySkill = _enum_auto('SkillType') 23 | ForceTorqueSkill = _enum_auto('SkillType') 24 | GripperSkill = _enum_auto('SkillType') 25 | ImpedanceControlSkill = _enum_auto('SkillType') 26 | JointPositionSkill = _enum_auto('SkillType') 27 | JointVelocitySkill = _enum_auto('SkillType') 28 | 29 | 30 | class MetaSkillType: 31 | BaseMetaSkill = _enum_auto('MetaSkillType') 32 | JointPositionContinuousSkill = _enum_auto('MetaSkillType') 33 | 34 | 35 | class TrajectoryGeneratorType: 36 | CartesianVelocityTrajectoryGenerator = _enum_auto('TrajectoryGeneratorType') 37 | CubicHermiteSplineJointTrajectoryGenerator = _enum_auto('TrajectoryGeneratorType') 38 | CubicHermiteSplinePoseTrajectoryGenerator = _enum_auto('TrajectoryGeneratorType') 39 | GoalPoseDmpTrajectoryGenerator = _enum_auto('TrajectoryGeneratorType') 40 | GripperTrajectoryGenerator = _enum_auto('TrajectoryGeneratorType') 41 | ImpulseTrajectoryGenerator = _enum_auto('TrajectoryGeneratorType') 42 | JointDmpTrajectoryGenerator = _enum_auto('TrajectoryGeneratorType') 43 | JointVelocityTrajectoryGenerator = _enum_auto('TrajectoryGeneratorType') 44 | LinearForcePositionTrajectoryGenerator = _enum_auto('TrajectoryGeneratorType') 45 | LinearJointTrajectoryGenerator = _enum_auto('TrajectoryGeneratorType') 46 | LinearPoseTrajectoryGenerator = _enum_auto('TrajectoryGeneratorType') 47 | MinJerkJointTrajectoryGenerator = _enum_auto('TrajectoryGeneratorType') 48 | MinJerkPoseTrajectoryGenerator = _enum_auto('TrajectoryGeneratorType') 49 | PassThroughCartesianVelocityTrajectoryGenerator = _enum_auto('TrajectoryGeneratorType') 50 | PassThroughForcePositionTrajectoryGenerator = _enum_auto('TrajectoryGeneratorType') 51 | PassThroughJointTrajectoryGenerator = _enum_auto('TrajectoryGeneratorType') 52 | PassThroughJointVelocityTrajectoryGenerator = _enum_auto('TrajectoryGeneratorType') 53 | PassThroughPoseTrajectoryGenerator = _enum_auto('TrajectoryGeneratorType') 54 | PoseDmpTrajectoryGenerator = _enum_auto('TrajectoryGeneratorType') 55 | QuaternionPoseDmpTrajectoryGenerator = _enum_auto('TrajectoryGeneratorType') 56 | RelativeLinearPoseTrajectoryGenerator = _enum_auto('TrajectoryGeneratorType') 57 | RelativeMinJerkPoseTrajectoryGenerator = _enum_auto('TrajectoryGeneratorType') 58 | SineJointTrajectoryGenerator = _enum_auto('TrajectoryGeneratorType') 59 | SinePoseTrajectoryGenerator = _enum_auto('TrajectoryGeneratorType') 60 | StayInInitialJointsTrajectoryGenerator = _enum_auto('TrajectoryGeneratorType') 61 | StayInInitialPoseTrajectoryGenerator = _enum_auto('TrajectoryGeneratorType') 62 | 63 | 64 | class FeedbackControllerType: 65 | CartesianImpedanceFeedbackController = _enum_auto('FeedbackControllerType') 66 | EECartesianImpedanceFeedbackController = _enum_auto('FeedbackControllerType') 67 | ForceAxisImpedenceFeedbackController = _enum_auto('FeedbackControllerType') 68 | ForcePositionFeedbackController = _enum_auto('FeedbackControllerType') 69 | JointImpedanceFeedbackController = _enum_auto('FeedbackControllerType') 70 | NoopFeedbackController = _enum_auto('FeedbackControllerType') 71 | PassThroughFeedbackController = _enum_auto('FeedbackControllerType') 72 | PassThroughJointTorqueFeedbackController = _enum_auto('FeedbackControllerType') 73 | SetInternalImpedanceFeedbackController = _enum_auto('FeedbackControllerType') 74 | 75 | 76 | class TerminationHandlerType: 77 | ContactTerminationHandler = _enum_auto('TerminationHandlerType') 78 | FinalJointTerminationHandler = _enum_auto('TerminationHandlerType') 79 | FinalPoseTerminationHandler = _enum_auto('TerminationHandlerType') 80 | NoopTerminationHandler = _enum_auto('TerminationHandlerType') 81 | TimeTerminationHandler = _enum_auto('TerminationHandlerType') 82 | 83 | 84 | class SkillStatus: 85 | TO_START = _enum_auto('SkillStatus') 86 | RUNNING = _enum_auto('SkillStatus') 87 | FINISHED = _enum_auto('SkillStatus') 88 | VIRT_COLL_ERR = _enum_auto('SkillStatus') 89 | 90 | 91 | class SensorDataMessageType: 92 | BOUNDING_BOX = _enum_auto('SensorDataMessageType') 93 | CARTESIAN_IMPEDANCE = _enum_auto('SensorDataMessageType') 94 | CARTESIAN_VELOCITY = _enum_auto('SensorDataMessageType') 95 | FORCE_POSITION = _enum_auto('SensorDataMessageType') 96 | FORCE_POSITION_GAINS = _enum_auto('SensorDataMessageType') 97 | JOINT_POSITION_VELOCITY = _enum_auto('SensorDataMessageType') 98 | JOINT_POSITION = _enum_auto('SensorDataMessageType') 99 | JOINT_VELOCITY = _enum_auto('SensorDataMessageType') 100 | JOINT_TORQUE = _enum_auto('SensorDataMessageType') 101 | POSE_POSITION_VELOCITY = _enum_auto('SensorDataMessageType') 102 | POSE_POSITION = _enum_auto('SensorDataMessageType') 103 | SHOULD_TERMINATE = _enum_auto('SensorDataMessageType') 104 | -------------------------------------------------------------------------------- /frankapy/proto/__init__.py: -------------------------------------------------------------------------------- 1 | from .feedback_controller_params_msg_pb2 import * 2 | from .sensor_msg_pb2 import * 3 | from .termination_handler_params_msg_pb2 import * 4 | from .trajectory_generator_params_msg_pb2 import * -------------------------------------------------------------------------------- /frankapy/proto/feedback_controller_params_msg.proto: -------------------------------------------------------------------------------- 1 | syntax = "proto2"; 2 | 3 | message CartesianImpedanceFeedbackControllerMessage { 4 | repeated double translational_stiffnesses = 1; 5 | repeated double rotational_stiffnesses = 2; 6 | } 7 | 8 | message ForceAxisFeedbackControllerMessage { 9 | required double translational_stiffness = 1; 10 | required double rotational_stiffness = 2; 11 | 12 | repeated double axis = 3; 13 | } 14 | 15 | message JointImpedanceFeedbackControllerMessage { 16 | repeated double k_gains = 1; 17 | repeated double d_gains = 2; 18 | } 19 | 20 | message InternalImpedanceFeedbackControllerMessage { 21 | repeated double cartesian_impedances = 1; 22 | repeated double joint_impedances = 2; 23 | } 24 | 25 | message ForcePositionFeedbackControllerMessage { 26 | repeated double position_kps_cart = 1; 27 | repeated double force_kps_cart = 2; 28 | repeated double position_kps_joint = 3; 29 | repeated double force_kps_joint = 4; 30 | repeated double selection = 5; 31 | required bool use_cartesian_gains = 6; 32 | } 33 | 34 | message JointTorqueFeedbackControllerMessage { 35 | repeated double selection = 1; 36 | repeated double remove_gravity = 2; 37 | repeated double joint_torques = 3; 38 | repeated double k_gains = 4; 39 | repeated double d_gains = 5; 40 | } 41 | -------------------------------------------------------------------------------- /frankapy/proto/feedback_controller_params_msg_pb2.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | # Generated by the protocol buffer compiler. DO NOT EDIT! 3 | # source: feedback_controller_params_msg.proto 4 | """Generated protocol buffer code.""" 5 | from google.protobuf.internal import builder as _builder 6 | from google.protobuf import descriptor as _descriptor 7 | from google.protobuf import descriptor_pool as _descriptor_pool 8 | from google.protobuf import symbol_database as _symbol_database 9 | # @@protoc_insertion_point(imports) 10 | 11 | _sym_db = _symbol_database.Default() 12 | 13 | 14 | 15 | 16 | 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') 17 | 18 | _builder.BuildMessageAndEnumDescriptors(DESCRIPTOR, globals()) 19 | _builder.BuildTopDescriptorsAndMessages(DESCRIPTOR, 'feedback_controller_params_msg_pb2', globals()) 20 | if _descriptor._USE_C_DESCRIPTORS == False: 21 | 22 | DESCRIPTOR._options = None 23 | _CARTESIANIMPEDANCEFEEDBACKCONTROLLERMESSAGE._serialized_start=40 24 | _CARTESIANIMPEDANCEFEEDBACKCONTROLLERMESSAGE._serialized_end=152 25 | _FORCEAXISFEEDBACKCONTROLLERMESSAGE._serialized_start=154 26 | _FORCEAXISFEEDBACKCONTROLLERMESSAGE._serialized_end=267 27 | _JOINTIMPEDANCEFEEDBACKCONTROLLERMESSAGE._serialized_start=269 28 | _JOINTIMPEDANCEFEEDBACKCONTROLLERMESSAGE._serialized_end=344 29 | _INTERNALIMPEDANCEFEEDBACKCONTROLLERMESSAGE._serialized_start=346 30 | _INTERNALIMPEDANCEFEEDBACKCONTROLLERMESSAGE._serialized_end=446 31 | _FORCEPOSITIONFEEDBACKCONTROLLERMESSAGE._serialized_start=449 32 | _FORCEPOSITIONFEEDBACKCONTROLLERMESSAGE._serialized_end=641 33 | _JOINTTORQUEFEEDBACKCONTROLLERMESSAGE._serialized_start=644 34 | _JOINTTORQUEFEEDBACKCONTROLLERMESSAGE._serialized_end=782 35 | # @@protoc_insertion_point(module_scope) 36 | -------------------------------------------------------------------------------- /frankapy/proto/sensor_msg.proto: -------------------------------------------------------------------------------- 1 | // franka-interface/proto/sensor_msg.proto and frankapy/protosensor_msg.proto should match 2 | 3 | syntax = "proto2"; 4 | 5 | message BoundingBox { 6 | required string name = 1; 7 | required int32 id = 2; 8 | 9 | required int32 x = 3; 10 | required int32 y = 4; 11 | required int32 w = 5; 12 | required int32 h = 6; 13 | } 14 | 15 | message JointPositionVelocitySensorMessage { 16 | required int32 id = 1; 17 | required double timestamp = 2; 18 | required double seg_run_time = 3; 19 | 20 | repeated double joints = 4; 21 | repeated double joint_vels = 5; 22 | } 23 | 24 | message PosePositionVelocitySensorMessage { 25 | required int32 id = 1; 26 | required double timestamp = 2; 27 | required double seg_run_time = 3; 28 | 29 | repeated double position = 4; 30 | repeated double quaternion = 5; 31 | repeated double pose_velocities = 6; 32 | } 33 | 34 | message JointVelocitySensorMessage { 35 | required int32 id = 1; 36 | required double timestamp = 2; 37 | 38 | repeated double joint_velocities = 3; 39 | } 40 | 41 | message JointPositionSensorMessage { 42 | required int32 id = 1; 43 | required double timestamp = 2; 44 | 45 | repeated double joints = 3; 46 | } 47 | 48 | message PosePositionSensorMessage { 49 | required int32 id = 1; 50 | required double timestamp = 2; 51 | 52 | repeated double position = 3; 53 | repeated double quaternion = 4; 54 | } 55 | 56 | message CartesianVelocitySensorMessage { 57 | required int32 id = 1; 58 | required double timestamp = 2; 59 | 60 | repeated double cartesian_velocities = 3; 61 | } 62 | 63 | message ShouldTerminateSensorMessage { 64 | required double timestamp = 1; 65 | required bool should_terminate = 2; 66 | } 67 | 68 | message CartesianImpedanceSensorMessage { 69 | required int32 id = 1; 70 | required double timestamp = 2; 71 | 72 | repeated double translational_stiffnesses = 3; 73 | repeated double rotational_stiffnesses = 4; 74 | } 75 | 76 | message ForcePositionSensorMessage { 77 | required int32 id = 1; 78 | required double timestamp = 2; 79 | required double seg_run_time = 3; 80 | 81 | repeated double pose = 4; 82 | repeated double force = 5; 83 | } 84 | 85 | message ForcePositionControllerSensorMessage { 86 | required int32 id = 1; 87 | required double timestamp = 2; 88 | 89 | repeated double position_kps_cart = 3; 90 | repeated double force_kps_cart = 4; 91 | repeated double position_kps_joint = 5; 92 | repeated double force_kps_joint = 6; 93 | repeated double selection = 7; 94 | } 95 | 96 | message JointTorqueControllerSensorMessage { 97 | required int32 id = 1; 98 | required double timestamp = 2; 99 | 100 | repeated double selection = 3; 101 | repeated double remove_gravity = 4; 102 | repeated double joint_torques = 5; 103 | repeated double k_gains = 6; 104 | repeated double d_gains = 7; 105 | } -------------------------------------------------------------------------------- /frankapy/proto/sensor_msg_pb2.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | # Generated by the protocol buffer compiler. DO NOT EDIT! 3 | # source: sensor_msg.proto 4 | """Generated protocol buffer code.""" 5 | from google.protobuf.internal import builder as _builder 6 | from google.protobuf import descriptor as _descriptor 7 | from google.protobuf import descriptor_pool as _descriptor_pool 8 | from google.protobuf import symbol_database as _symbol_database 9 | # @@protoc_insertion_point(imports) 10 | 11 | _sym_db = _symbol_database.Default() 12 | 13 | 14 | 15 | 16 | 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') 17 | 18 | _builder.BuildMessageAndEnumDescriptors(DESCRIPTOR, globals()) 19 | _builder.BuildTopDescriptorsAndMessages(DESCRIPTOR, 'sensor_msg_pb2', globals()) 20 | if _descriptor._USE_C_DESCRIPTORS == False: 21 | 22 | DESCRIPTOR._options = None 23 | _BOUNDINGBOX._serialized_start=20 24 | _BOUNDINGBOX._serialized_end=103 25 | _JOINTPOSITIONVELOCITYSENSORMESSAGE._serialized_start=105 26 | _JOINTPOSITIONVELOCITYSENSORMESSAGE._serialized_end=230 27 | _POSEPOSITIONVELOCITYSENSORMESSAGE._serialized_start=233 28 | _POSEPOSITIONVELOCITYSENSORMESSAGE._serialized_end=384 29 | _JOINTVELOCITYSENSORMESSAGE._serialized_start=386 30 | _JOINTVELOCITYSENSORMESSAGE._serialized_end=471 31 | _JOINTPOSITIONSENSORMESSAGE._serialized_start=473 32 | _JOINTPOSITIONSENSORMESSAGE._serialized_end=548 33 | _POSEPOSITIONSENSORMESSAGE._serialized_start=550 34 | _POSEPOSITIONSENSORMESSAGE._serialized_end=646 35 | _CARTESIANVELOCITYSENSORMESSAGE._serialized_start=648 36 | _CARTESIANVELOCITYSENSORMESSAGE._serialized_end=741 37 | _SHOULDTERMINATESENSORMESSAGE._serialized_start=743 38 | _SHOULDTERMINATESENSORMESSAGE._serialized_end=818 39 | _CARTESIANIMPEDANCESENSORMESSAGE._serialized_start=821 40 | _CARTESIANIMPEDANCESENSORMESSAGE._serialized_end=952 41 | _FORCEPOSITIONSENSORMESSAGE._serialized_start=954 42 | _FORCEPOSITIONSENSORMESSAGE._serialized_end=1064 43 | _FORCEPOSITIONCONTROLLERSENSORMESSAGE._serialized_start=1067 44 | _FORCEPOSITIONCONTROLLERSENSORMESSAGE._serialized_end=1259 45 | _JOINTTORQUECONTROLLERSENSORMESSAGE._serialized_start=1262 46 | _JOINTTORQUECONTROLLERSENSORMESSAGE._serialized_end=1429 47 | # @@protoc_insertion_point(module_scope) 48 | -------------------------------------------------------------------------------- /frankapy/proto/termination_handler_params_msg.proto: -------------------------------------------------------------------------------- 1 | syntax = "proto2"; 2 | 3 | message ContactTerminationHandlerMessage { 4 | required double buffer_time = 1; 5 | 6 | repeated double force_thresholds = 2; 7 | repeated double torque_thresholds = 3; 8 | } 9 | 10 | message JointThresholdMessage { 11 | required double buffer_time = 1; 12 | 13 | repeated double joint_thresholds = 2; 14 | } 15 | 16 | message PoseThresholdMessage { 17 | required double buffer_time = 1; 18 | 19 | repeated double position_thresholds = 2; 20 | repeated double orientation_thresholds = 3; 21 | } 22 | 23 | message TimeTerminationHandlerMessage { 24 | required double buffer_time = 1; 25 | } -------------------------------------------------------------------------------- /frankapy/proto/termination_handler_params_msg_pb2.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | # Generated by the protocol buffer compiler. DO NOT EDIT! 3 | # source: termination_handler_params_msg.proto 4 | """Generated protocol buffer code.""" 5 | from google.protobuf.internal import builder as _builder 6 | from google.protobuf import descriptor as _descriptor 7 | from google.protobuf import descriptor_pool as _descriptor_pool 8 | from google.protobuf import symbol_database as _symbol_database 9 | # @@protoc_insertion_point(imports) 10 | 11 | _sym_db = _symbol_database.Default() 12 | 13 | 14 | 15 | 16 | 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') 17 | 18 | _builder.BuildMessageAndEnumDescriptors(DESCRIPTOR, globals()) 19 | _builder.BuildTopDescriptorsAndMessages(DESCRIPTOR, 'termination_handler_params_msg_pb2', globals()) 20 | if _descriptor._USE_C_DESCRIPTORS == False: 21 | 22 | DESCRIPTOR._options = None 23 | _CONTACTTERMINATIONHANDLERMESSAGE._serialized_start=40 24 | _CONTACTTERMINATIONHANDLERMESSAGE._serialized_end=148 25 | _JOINTTHRESHOLDMESSAGE._serialized_start=150 26 | _JOINTTHRESHOLDMESSAGE._serialized_end=220 27 | _POSETHRESHOLDMESSAGE._serialized_start=222 28 | _POSETHRESHOLDMESSAGE._serialized_end=326 29 | _TIMETERMINATIONHANDLERMESSAGE._serialized_start=328 30 | _TIMETERMINATIONHANDLERMESSAGE._serialized_end=380 31 | # @@protoc_insertion_point(module_scope) 32 | -------------------------------------------------------------------------------- /frankapy/proto/trajectory_generator_params_msg.proto: -------------------------------------------------------------------------------- 1 | syntax = "proto2"; 2 | 3 | message GripperTrajectoryGeneratorMessage { 4 | required bool grasp = 1; 5 | required double width = 2; 6 | required double speed = 3; 7 | required double force = 4; 8 | } 9 | 10 | message ImpulseTrajectoryGeneratorMessage { 11 | required double run_time = 1; 12 | required double acc_time = 2; 13 | required double max_trans = 3; 14 | required double max_rot = 4; 15 | 16 | repeated double forces = 5; 17 | repeated double torques = 6; 18 | } 19 | 20 | message JointTrajectoryGeneratorMessage { 21 | required double run_time = 1; 22 | 23 | repeated double joints = 2; 24 | } 25 | 26 | message JointVelocityTrajectoryGeneratorMessage { 27 | required double run_time = 1; 28 | 29 | repeated double joint_velocities = 2; 30 | repeated double joint_accelerations = 3; 31 | } 32 | 33 | message CartesianVelocityTrajectoryGeneratorMessage { 34 | required double run_time = 1; 35 | 36 | repeated double cartesian_velocities = 2; 37 | repeated double cartesian_accelerations = 3; 38 | } 39 | 40 | message PoseTrajectoryGeneratorMessage { 41 | required double run_time = 1; 42 | 43 | repeated double position = 2; 44 | repeated double quaternion = 3; 45 | repeated double pose = 4; 46 | } 47 | 48 | message JointDMPTrajectoryGeneratorMessage { 49 | required double run_time = 1; 50 | required double tau = 2; 51 | required double alpha = 3; 52 | required double beta = 4; 53 | required double num_basis = 5; 54 | required double num_sensor_values = 6; 55 | 56 | repeated double basis_mean = 7; 57 | repeated double basis_std = 8; 58 | repeated double weights = 9; 59 | repeated double initial_sensor_values = 10; 60 | } 61 | 62 | message PoseDMPTrajectoryGeneratorMessage { 63 | required bool orientation_only = 1; 64 | required bool position_only = 2; 65 | required bool ee_frame = 3; 66 | required double run_time = 4; 67 | required double tau = 5; 68 | required double alpha = 6; 69 | required double beta = 7; 70 | required double num_basis = 8; 71 | required double num_sensor_values = 9; 72 | 73 | repeated double basis_mean = 10; 74 | repeated double basis_std = 11; 75 | repeated double weights = 12; 76 | repeated double initial_sensor_values = 13; 77 | } 78 | 79 | message QuaternionPoseDMPTrajectoryGeneratorMessage { 80 | required bool ee_frame = 1; 81 | required double run_time = 2; 82 | required double tau_pos = 3; 83 | required double alpha_pos = 4; 84 | required double beta_pos = 5; 85 | required double tau_quat = 6; 86 | required double alpha_quat = 7; 87 | required double beta_quat = 8; 88 | 89 | required double num_basis_pos = 9; 90 | required double num_sensor_values_pos = 10; 91 | required double num_basis_quat = 11; 92 | required double num_sensor_values_quat = 12; 93 | 94 | repeated double pos_basis_mean = 13; 95 | repeated double pos_basis_std = 14; 96 | repeated double pos_weights = 15; 97 | repeated double pos_initial_sensor_values = 16; 98 | 99 | repeated double quat_basis_mean = 17; 100 | repeated double quat_basis_std = 18; 101 | repeated double quat_weights = 19; 102 | repeated double quat_initial_sensor_values = 20; 103 | 104 | required double goal_time_quat = 21; 105 | required double goal_quat_w = 22; 106 | required double goal_quat_x = 23; 107 | required double goal_quat_y = 24; 108 | required double goal_quat_z = 25; 109 | } 110 | 111 | message RunTimeMessage { 112 | required double run_time = 1; 113 | } -------------------------------------------------------------------------------- /frankapy/proto/trajectory_generator_params_msg_pb2.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | # Generated by the protocol buffer compiler. DO NOT EDIT! 3 | # source: trajectory_generator_params_msg.proto 4 | """Generated protocol buffer code.""" 5 | from google.protobuf.internal import builder as _builder 6 | from google.protobuf import descriptor as _descriptor 7 | from google.protobuf import descriptor_pool as _descriptor_pool 8 | from google.protobuf import symbol_database as _symbol_database 9 | # @@protoc_insertion_point(imports) 10 | 11 | _sym_db = _symbol_database.Default() 12 | 13 | 14 | 15 | 16 | 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') 17 | 18 | _builder.BuildMessageAndEnumDescriptors(DESCRIPTOR, globals()) 19 | _builder.BuildTopDescriptorsAndMessages(DESCRIPTOR, 'trajectory_generator_params_msg_pb2', globals()) 20 | if _descriptor._USE_C_DESCRIPTORS == False: 21 | 22 | DESCRIPTOR._options = None 23 | _GRIPPERTRAJECTORYGENERATORMESSAGE._serialized_start=41 24 | _GRIPPERTRAJECTORYGENERATORMESSAGE._serialized_end=136 25 | _IMPULSETRAJECTORYGENERATORMESSAGE._serialized_start=139 26 | _IMPULSETRAJECTORYGENERATORMESSAGE._serialized_end=279 27 | _JOINTTRAJECTORYGENERATORMESSAGE._serialized_start=281 28 | _JOINTTRAJECTORYGENERATORMESSAGE._serialized_end=348 29 | _JOINTVELOCITYTRAJECTORYGENERATORMESSAGE._serialized_start=350 30 | _JOINTVELOCITYTRAJECTORYGENERATORMESSAGE._serialized_end=464 31 | _CARTESIANVELOCITYTRAJECTORYGENERATORMESSAGE._serialized_start=466 32 | _CARTESIANVELOCITYTRAJECTORYGENERATORMESSAGE._serialized_end=592 33 | _POSETRAJECTORYGENERATORMESSAGE._serialized_start=594 34 | _POSETRAJECTORYGENERATORMESSAGE._serialized_end=696 35 | _JOINTDMPTRAJECTORYGENERATORMESSAGE._serialized_start=699 36 | _JOINTDMPTRAJECTORYGENERATORMESSAGE._serialized_end=928 37 | _POSEDMPTRAJECTORYGENERATORMESSAGE._serialized_start=931 38 | _POSEDMPTRAJECTORYGENERATORMESSAGE._serialized_end=1226 39 | _QUATERNIONPOSEDMPTRAJECTORYGENERATORMESSAGE._serialized_start=1229 40 | _QUATERNIONPOSEDMPTRAJECTORYGENERATORMESSAGE._serialized_end=1849 41 | _RUNTIMEMESSAGE._serialized_start=1851 42 | _RUNTIMEMESSAGE._serialized_end=1885 43 | # @@protoc_insertion_point(module_scope) 44 | -------------------------------------------------------------------------------- /frankapy/proto_utils.py: -------------------------------------------------------------------------------- 1 | from franka_interface_msgs.msg import SensorData, SensorDataGroup 2 | 3 | 4 | def sensor_proto2ros_msg(sensor_proto_msg, sensor_data_type, info=''): 5 | sensor_ros_msg = SensorData() 6 | sensor_ros_msg.type = sensor_data_type 7 | sensor_ros_msg.info = info 8 | 9 | sensor_data_bytes = sensor_proto_msg.SerializeToString() 10 | sensor_ros_msg.size = len(sensor_data_bytes) 11 | sensor_ros_msg.sensorData = sensor_data_bytes 12 | 13 | return sensor_ros_msg 14 | 15 | 16 | def make_sensor_group_msg(trajectory_generator_sensor_msg=None, feedback_controller_sensor_msg=None, termination_handler_sensor_msg=None): 17 | sensor_group_msg = SensorDataGroup() 18 | 19 | if trajectory_generator_sensor_msg is not None: 20 | sensor_group_msg.has_trajectory_generator_sensor_data = True 21 | sensor_group_msg.trajectoryGeneratorSensorData = trajectory_generator_sensor_msg 22 | if feedback_controller_sensor_msg is not None: 23 | sensor_group_msg.has_feedback_controller_sensor_data = True 24 | sensor_group_msg.feedbackControllerSensorData = feedback_controller_sensor_msg 25 | if termination_handler_sensor_msg is not None: 26 | sensor_group_msg.has_termination_handler_sensor_data = True 27 | sensor_group_msg.terminationHandlerSensorData = termination_handler_sensor_msg 28 | 29 | return sensor_group_msg 30 | -------------------------------------------------------------------------------- /frankapy/ros_utils.py: -------------------------------------------------------------------------------- 1 | import rospy 2 | from visualization_msgs.msg import Marker, MarkerArray 3 | import quaternion 4 | 5 | 6 | class BoxesPublisher: 7 | 8 | def __init__(self, name, world_frame='panda_link0'): 9 | self._boxes_pub = rospy.Publisher(name, MarkerArray, queue_size=10) 10 | self._world_frame = world_frame 11 | 12 | def publish_boxes(self, boxes): 13 | markers = [] 14 | for i, box in enumerate(boxes): 15 | marker = Marker() 16 | marker.type = Marker.CUBE 17 | marker.header.stamp = rospy.Time.now() 18 | marker.header.frame_id = self._world_frame 19 | marker.id = i 20 | 21 | marker.lifetime = rospy.Duration() 22 | 23 | marker.pose.position.x = box[0] 24 | marker.pose.position.y = box[1] 25 | marker.pose.position.z = box[2] 26 | 27 | marker.scale.x = box[-3] 28 | marker.scale.y = box[-2] 29 | marker.scale.z = box[-1] 30 | 31 | if len(box) == 9: 32 | q = quaternion.from_euler_angles(box[3], box[4], box[5]) 33 | for k in 'wxyz': 34 | setattr(marker.pose.orientation, k, getattr(q, k)) 35 | elif len(box) == 10: 36 | for j, k in enumerate('wxyz'): 37 | setattr(marker.pose.orientation, k, box[3 + j]) 38 | else: 39 | raise ValueError('Invalid format for box!') 40 | 41 | marker.color.r = 0.5 42 | marker.color.g = 0.5 43 | marker.color.b = 0.5 44 | marker.color.a = 0.6 45 | 46 | markers.append(marker) 47 | 48 | marker_array = MarkerArray(markers) 49 | self._boxes_pub.publish(marker_array) -------------------------------------------------------------------------------- /frankapy/skill_list.py: -------------------------------------------------------------------------------- 1 | import roslib 2 | roslib.load_manifest('franka_interface_msgs') 3 | import rospy 4 | import actionlib 5 | import numpy as np 6 | from autolab_core import RigidTransform 7 | 8 | from .franka_constants import FrankaConstants as FC 9 | from .franka_interface_common_definitions import * 10 | from .proto import * 11 | from .utils import transform_to_list 12 | 13 | from franka_interface_msgs.msg import ExecuteSkillAction, ExecuteSkillGoal 14 | 15 | class Skill: 16 | def __init__(self, 17 | skill_type, 18 | trajectory_generator_type, 19 | feedback_controller_type = FeedbackControllerType.NoopFeedbackController, 20 | termination_handler_type = TerminationHandlerType.NoopTerminationHandler, 21 | meta_skill_type = MetaSkillType.BaseMetaSkill, 22 | meta_skill_id = 0, 23 | sensor_topics = None, 24 | timer_type = 1, 25 | skill_desc = ''): 26 | self._skill_type = skill_type 27 | self._skill_desc = skill_desc 28 | self._meta_skill_type = meta_skill_type 29 | self._meta_skill_id = meta_skill_id 30 | self._sensor_topics = sensor_topics if sensor_topics is not None else [''] 31 | self._trajectory_generator_type = trajectory_generator_type 32 | self._feedback_controller_type = feedback_controller_type 33 | self._termination_handler_type = termination_handler_type 34 | self._timer_type = timer_type 35 | 36 | self._sensor_value_sizes = 0 37 | self._initial_sensor_values = [] 38 | 39 | # Add trajectory params 40 | self._trajectory_generator_param_data = [] 41 | self._trajectory_generator_param_data_size = 0 42 | 43 | # Add feedback controller params 44 | self._feedback_controller_param_data = [] 45 | self._feedback_controller_param_data_size = 0 46 | 47 | # Add termination params 48 | self._termination_handler_param_data = [] 49 | self._termination_handler_param_data_size = 0 50 | 51 | # Add timer params 52 | self._timer_params = [] 53 | self._num_timer_params = 0 54 | 55 | def set_meta_skill_type(self, meta_skill_type): 56 | self._meta_skill_type = meta_skill_type 57 | 58 | def set_meta_skill_id(self, meta_skill_id): 59 | self._meta_skill_id = meta_skill_id 60 | 61 | def add_initial_sensor_values(self, values): 62 | self._initial_sensor_values = values 63 | self._sensor_value_sizes = [len(values)] 64 | 65 | def add_trajectory_params(self, params): 66 | self._trajectory_generator_param_data = params 67 | self._trajectory_generator_param_data_size = len(params) 68 | 69 | def add_feedback_controller_params(self, params): 70 | self._feedback_controller_param_data = params 71 | self._feedback_controller_param_data_size = len(params) 72 | 73 | def add_termination_params(self, params): 74 | self._termination_handler_param_data = params 75 | self._termination_handler_param_data_size = len(params) 76 | 77 | def add_timer_params(self, params): 78 | self._timer_params = params 79 | self._num_timer_params = len(params) 80 | 81 | ## Feedback Controllers 82 | 83 | def set_cartesian_impedances(self, use_impedance, cartesian_impedances, joint_impedances): 84 | if use_impedance: 85 | if cartesian_impedances is not None: 86 | self.add_cartesian_impedances(cartesian_impedances) 87 | else: 88 | self.add_cartesian_impedances(FC.DEFAULT_TRANSLATIONAL_STIFFNESSES + FC.DEFAULT_ROTATIONAL_STIFFNESSES) 89 | else: 90 | if joint_impedances is not None: 91 | self.add_internal_impedances([], joint_impedances) 92 | elif cartesian_impedances is not None: 93 | self.add_internal_impedances(cartesian_impedances, []) 94 | else: 95 | self.add_internal_impedances([], FC.DEFAULT_JOINT_IMPEDANCES) 96 | 97 | def set_joint_impedances(self, use_impedance, cartesian_impedances, joint_impedances, k_gains, d_gains): 98 | if use_impedance: 99 | if k_gains is not None and d_gains is not None: 100 | self.add_joint_gains(k_gains, d_gains) 101 | else: 102 | self.add_joint_gains(FC.DEFAULT_K_GAINS, FC.DEFAULT_D_GAINS) 103 | else: 104 | if joint_impedances is not None: 105 | self.add_internal_impedances([], joint_impedances) 106 | elif cartesian_impedances is not None: 107 | self.add_internal_impedances(cartesian_impedances, []) 108 | else: 109 | self.add_internal_impedances([], FC.DEFAULT_JOINT_IMPEDANCES) 110 | 111 | def add_cartesian_impedances(self, cartesian_impedances): 112 | assert type(cartesian_impedances) is list, \ 113 | "Incorrect cartesian impedances type. Should be list." 114 | assert len(cartesian_impedances) == 6, \ 115 | "Incorrect cartesian impedances len. Should be 6." 116 | assert self._skill_type == SkillType.ImpedanceControlSkill, \ 117 | "Incorrect skill type. Should be ImpedanceControlSkill." 118 | 119 | cartesian_impedance_feedback_controller_msg_proto = \ 120 | CartesianImpedanceFeedbackControllerMessage( 121 | translational_stiffnesses=cartesian_impedances[:3], 122 | rotational_stiffnesses=cartesian_impedances[3:]) 123 | 124 | self.add_feedback_controller_params(cartesian_impedance_feedback_controller_msg_proto.SerializeToString()) 125 | 126 | def add_force_axis_params(self, translational_stiffness, rotational_stiffness, axis): 127 | assert type(translational_stiffness) is float or type(translational_stiffness) is int, \ 128 | "Incorrect translational stiffness type. Should be int or float." 129 | assert type(rotational_stiffness) is float or type(rotational_stiffness) is int, \ 130 | "Incorrect rotational stiffness type. Should be int or float." 131 | assert type(axis) is list, \ 132 | "Incorrect axis type. Should be list." 133 | assert len(axis) == 3, \ 134 | "Incorrect axis len. Should be 3." 135 | 136 | force_axis_feedback_controller_msg_proto = \ 137 | ForceAxisFeedbackControllerMessage(translational_stiffness=translational_stiffness, 138 | rotational_stiffness=rotational_stiffness, 139 | axis=axis) 140 | 141 | self.add_feedback_controller_params(force_axis_feedback_controller_msg_proto.SerializeToString()) 142 | 143 | def add_internal_impedances(self, cartesian_impedances, joint_impedances): 144 | assert type(cartesian_impedances) is list, \ 145 | "Incorrect joint impedances type. Should be list." 146 | assert type(joint_impedances) is list, \ 147 | "Incorrect joint impedances type. Should be list." 148 | assert len(cartesian_impedances) == 0 or len(cartesian_impedances) == 6, \ 149 | "Incorrect cartesian impedances len. Should be 0 or 6." 150 | assert len(joint_impedances) == 0 or len(joint_impedances) == 7, \ 151 | "Incorrect joint impedances len. Should be 0 or 7." 152 | assert self._skill_type == SkillType.CartesianPoseSkill or self._skill_type == SkillType.JointPositionSkill \ 153 | or self._skill_type == SkillType.CartesianVelocitySkill or self._skill_type == SkillType.JointVelocitySkill, \ 154 | "Incorrect skill type. Should be CartesianPoseSkill, JointPositionSkill, CartesianVelocitySkill, or JointVelocitySkill." 155 | internal_feedback_controller_msg_proto = \ 156 | InternalImpedanceFeedbackControllerMessage( 157 | cartesian_impedances=cartesian_impedances, joint_impedances=joint_impedances) 158 | 159 | self.add_feedback_controller_params(internal_feedback_controller_msg_proto.SerializeToString()) 160 | 161 | def add_joint_torques(self, joint_torques, selection, remove_gravity, k_gains, d_gains): 162 | assert type(joint_torques) is list, "Incorrect joint_torques type. Should be list." 163 | assert type(selection) is list, "Incorrect selection type. Should be list." 164 | assert type(remove_gravity) is list, "Incorrect remove_gravity type. Should be list." 165 | assert type(k_gains) is list, "Incorrect k_gains type. Should be list." 166 | assert type(d_gains) is list, "Incorrect d_gains type. Should be list." 167 | assert len(joint_torques) == 7, "Incorrect joint_torques len. Should be 7." 168 | assert len(selection) == 7, "Incorrect selection len. Should be 7." 169 | assert len(remove_gravity) == 7, "Incorrect remove_gravity len. Should be 7." 170 | assert len(k_gains) == 7, "Incorrect k_gains len. Should be 7." 171 | assert len(d_gains) == 7, "Incorrect d_gains len. Should be 7." 172 | assert self._skill_type == SkillType.ImpedanceControlSkill, \ 173 | "Incorrect skill type. Should be ImpedanceControlSkill" 174 | 175 | joint_torque_controller_msg_proto = \ 176 | JointTorqueFeedbackControllerMessage(joint_torques=joint_torques, selection=selection, remove_gravity=remove_gravity, k_gains=k_gains, d_gains=d_gains) 177 | 178 | self.add_feedback_controller_params(joint_torque_controller_msg_proto.SerializeToString()) 179 | 180 | def add_force_position_params(self, position_kps_cart, force_kps_cart, position_kps_joint, force_kps_joint, S, use_cartesian_gains): 181 | assert type(position_kps_cart) is list or len(position_kps_cart) == 6, \ 182 | "Incorrect position_kps_cart type. Should be list of length 6." 183 | assert type(force_kps_cart) is list or len(force_kps_cart) == 6, \ 184 | "Incorrect force_kps_cart type. Should be list of length 6." 185 | assert type(position_kps_joint) is list or len(position_kps_joint) == 7, \ 186 | "Incorrect position_kps_joint type. Should be list of length 7." 187 | assert type(force_kps_joint) is list or len(force_kps_joint) == 7, \ 188 | "Incorrect force_kps_joint type. Should be list of length 7." 189 | assert type(S) is list and len(S) == 6, \ 190 | "Incorrect S type. Should be list of length 6." 191 | assert type(use_cartesian_gains) is bool, \ 192 | "Incorrect use_cartesian_gains type. Should be bool." 193 | 194 | force_position_feedback_controller_msg_proto = \ 195 | ForcePositionFeedbackControllerMessage( 196 | position_kps_cart=position_kps_cart, force_kps_cart=force_kps_cart, 197 | position_kps_joint=position_kps_joint, force_kps_joint=force_kps_joint, 198 | selection=S, use_cartesian_gains=use_cartesian_gains) 199 | 200 | self.add_feedback_controller_params(force_position_feedback_controller_msg_proto.SerializeToString()) 201 | 202 | def add_joint_gains(self, k_gains, d_gains): 203 | assert type(k_gains) is list, "Incorrect k_gains type. Should be list." 204 | assert type(d_gains) is list, "Incorrect d_gains type. Should be list." 205 | assert len(k_gains) == 7, "Incorrect k_gains len. Should be 7." 206 | assert len(d_gains) == 7, "Incorrect d_gains len. Should be 7." 207 | assert self._skill_type == SkillType.ImpedanceControlSkill, \ 208 | "Incorrect skill type. Should be ImpedanceControlSkill" 209 | 210 | joint_feedback_controller_msg_proto = \ 211 | JointImpedanceFeedbackControllerMessage(k_gains=k_gains, d_gains=d_gains) 212 | 213 | self.add_feedback_controller_params(joint_feedback_controller_msg_proto.SerializeToString()) 214 | 215 | ## Termination Handlers 216 | 217 | def check_for_contact_params(self, buffer_time, force_thresholds, torque_thresholds): 218 | if force_thresholds is not None or torque_thresholds is not None: 219 | self._termination_handler_type = TerminationHandlerType.ContactTerminationHandler 220 | 221 | if force_thresholds is None: 222 | force_thresholds = [] 223 | if torque_thresholds is None: 224 | torque_thresholds = [] 225 | self.add_contact_termination_params(buffer_time, 226 | force_thresholds, 227 | torque_thresholds) 228 | return True 229 | else: 230 | return False 231 | 232 | def add_contact_termination_params(self, buffer_time, 233 | force_thresholds, 234 | torque_thresholds): 235 | assert type(buffer_time) is float or type(buffer_time) is int, \ 236 | "Incorrect buffer time type. Should be int or float." 237 | assert buffer_time >= 0, "Incorrect buffer time. Should be non negative." 238 | assert type(force_thresholds) is list, \ 239 | "Incorrect force thresholds type. Should be list." 240 | assert type(torque_thresholds) is list, \ 241 | "Incorrect torque thresholds type. Should be list." 242 | assert len(force_thresholds) == 0 or len(force_thresholds) == 6, \ 243 | "Incorrect force thresholds length. Should be 0 or 6." 244 | assert len(torque_thresholds) == 0 or len(torque_thresholds) == 7, \ 245 | "Incorrect torque thresholds length. Should be 0 or 7." 246 | assert self._termination_handler_type == TerminationHandlerType.ContactTerminationHandler, \ 247 | "Incorrect termination handler type. Should be ContactTerminationHandler" 248 | 249 | contact_termination_handler_msg_proto = \ 250 | ContactTerminationHandlerMessage(buffer_time=buffer_time, force_thresholds=force_thresholds, 251 | torque_thresholds=torque_thresholds) 252 | 253 | self.add_termination_params(contact_termination_handler_msg_proto.SerializeToString()) 254 | 255 | def add_joint_threshold_params(self, buffer_time, joint_thresholds): 256 | assert type(buffer_time) is float or type(buffer_time) is int, \ 257 | "Incorrect buffer time type. Should be int or float." 258 | assert buffer_time >= 0, "Incorrect buffer time. Should be non negative." 259 | assert type(joint_thresholds) is list, \ 260 | "Incorrect joint thresholds type. Should be list." 261 | assert len(joint_thresholds) == 0 or len(joint_thresholds) == 7, \ 262 | "Incorrect joint thresholds length. Should be 0 or 7." 263 | assert self._termination_handler_type == TerminationHandlerType.FinalJointTerminationHandler, \ 264 | "Incorrect termination handler type. Should be FinalJointTerminationHandler" 265 | 266 | joint_threshold_msg_proto = JointThresholdMessage(buffer_time=buffer_time, 267 | joint_thresholds=joint_thresholds) 268 | 269 | self.add_termination_params(joint_threshold_msg_proto.SerializeToString()) 270 | 271 | def add_pose_threshold_params(self, buffer_time, pose_thresholds): 272 | assert type(buffer_time) is float or type(buffer_time) is int, \ 273 | "Incorrect buffer time type. Should be int or float." 274 | assert buffer_time >= 0, "Incorrect buffer time. Should be non negative." 275 | assert type(pose_thresholds) is list, \ 276 | "Incorrect pose thresholds type. Should be list." 277 | assert len(pose_thresholds) == 0 or len(pose_thresholds) == 6, \ 278 | "Incorrect pose thresholds length. Should be 0 or 6." 279 | assert self._termination_handler_type == TerminationHandlerType.FinalPoseTerminationHandler, \ 280 | "Incorrect termination handler type. Should be FinalPoseTerminationHandler" 281 | 282 | pose_threshold_msg_proto = PoseThresholdMessage(buffer_time=buffer_time, 283 | position_thresholds=pose_thresholds[:3], 284 | orientation_thresholds=pose_thresholds[3:]) 285 | 286 | self.add_termination_params(pose_threshold_msg_proto.SerializeToString()) 287 | 288 | def add_time_termination_params(self, buffer_time): 289 | assert type(buffer_time) is float or type(buffer_time) is int, \ 290 | "Incorrect buffer time type. Should be int or float." 291 | assert buffer_time >= 0, "Incorrect buffer time. Should be non negative." 292 | assert self._termination_handler_type == TerminationHandlerType.TimeTerminationHandler, \ 293 | "Incorrect termination handler type. Should be TimeTerminationHandler" 294 | 295 | time_termination_handler_msg_proto = TimeTerminationHandlerMessage(buffer_time=buffer_time) 296 | 297 | self.add_termination_params(time_termination_handler_msg_proto.SerializeToString()) 298 | 299 | ## Trajectory Generators 300 | 301 | def add_gripper_params(self, grasp, width, speed, force): 302 | assert type(grasp) is bool, \ 303 | "Incorrect grasp type. Should be bool." 304 | assert type(width) is float or type(width) is int, \ 305 | "Incorrect width type. Should be int or float." 306 | assert type(speed) is float or type(speed) is int, \ 307 | "Incorrect speed type. Should be int or float." 308 | assert type(force) is float or type(force) is int, \ 309 | "Incorrect force type. Should be int or float." 310 | assert width >= 0, "Incorrect width. Should be non negative." 311 | assert speed >= 0, "Incorrect speed. Should be non negative." 312 | assert force >= 0, "Incorrect force. Should be non negative." 313 | assert self._skill_type == SkillType.GripperSkill, \ 314 | "Incorrect skill type. Should be GripperSkill" 315 | assert self._trajectory_generator_type == TrajectoryGeneratorType.GripperTrajectoryGenerator, \ 316 | "Incorrect trajectory generator type. Should be GripperTrajectoryGenerator" 317 | 318 | gripper_trajectory_generator_msg_proto = GripperTrajectoryGeneratorMessage( 319 | grasp=grasp, width=width, speed=speed, force=force) 320 | 321 | self.add_trajectory_params(gripper_trajectory_generator_msg_proto.SerializeToString()) 322 | 323 | def add_impulse_params(self, run_time, acc_time, max_trans, max_rot, forces, torques): 324 | assert type(run_time) is float or type(run_time) is int, \ 325 | "Incorrect run_time type. Should be int or float." 326 | assert run_time >= 0, "Incorrect run_time. Should be non negative." 327 | assert type(acc_time) is float or type(acc_time) is int, \ 328 | "Incorrect acc time type. Should be int or float." 329 | assert acc_time >= 0, "Incorrect acc time. Should be non negative." 330 | assert type(max_trans) is float or type(max_trans) is int, \ 331 | "Incorrect max trans type. Should be int or float." 332 | assert max_trans >= 0, "Incorrect max trans. Should be non negative." 333 | assert type(max_rot) is float or type(max_rot) is int, \ 334 | "Incorrect max rot type. Should be int or float." 335 | assert max_rot >= 0, "Incorrect max rot. Should be non negative." 336 | assert type(forces) is list, "Incorrect forces type. Should be list." 337 | assert len(forces) == 3, "Incorrect forces len. Should be 3." 338 | assert type(torques) is list, "Incorrect torques type. Should be list." 339 | assert len(torques) == 3, "Incorrect torques len. Should be 3." 340 | 341 | impulse_trajectory_generator_msg_proto = ImpulseTrajectoryGeneratorMessage( 342 | run_time=run_time, acc_time=acc_time, max_trans=max_trans, 343 | max_rot=max_rot, forces=forces, torques=torques) 344 | 345 | self.add_trajectory_params(impulse_trajectory_generator_msg_proto.SerializeToString()) 346 | 347 | def add_goal_cartesian_velocities(self, run_time, cartesian_velocities, cartesian_accelerations): 348 | assert type(run_time) is float or type(run_time) is int,\ 349 | "Incorrect time type. Should be int or float." 350 | assert run_time >= 0, "Incorrect time. Should be non negative." 351 | assert type(cartesian_velocities) is list, "Incorrect cartesian_velocities type. Should be list." 352 | assert len(cartesian_velocities) == 6, "Incorrect cartesian_velocities len. Should be 7." 353 | assert type(cartesian_accelerations) is list, "Incorrect cartesian_accelerations type. Should be list." 354 | assert len(cartesian_accelerations) == 6, "Incorrect cartesian_accelerations len. Should be 7." 355 | 356 | cartesian_velocity_trajectory_generator_msg_proto = CartesianVelocityTrajectoryGeneratorMessage(run_time=run_time, 357 | cartesian_velocities=cartesian_velocities, cartesian_accelerations=cartesian_accelerations) 358 | 359 | self.add_trajectory_params(cartesian_velocity_trajectory_generator_msg_proto.SerializeToString()) 360 | 361 | def add_goal_pose(self, run_time, goal_pose): 362 | assert type(run_time) is float or type(run_time) is int, \ 363 | "Incorrect run_time type. Should be int or float." 364 | assert run_time >= 0, "Incorrect run_time. Should be non negative." 365 | assert type(goal_pose) is RigidTransform, "Incorrect goal_pose type. Should be RigidTransform." 366 | 367 | pose_trajectory_generator_msg_proto = PoseTrajectoryGeneratorMessage( 368 | run_time=run_time, position=goal_pose.translation, quaternion=goal_pose.quaternion, 369 | pose=transform_to_list(goal_pose)) 370 | 371 | self.add_trajectory_params(pose_trajectory_generator_msg_proto.SerializeToString()) 372 | 373 | def add_goal_joints(self, run_time, joints): 374 | assert type(run_time) is float or type(run_time) is int,\ 375 | "Incorrect time type. Should be int or float." 376 | assert run_time >= 0, "Incorrect time. Should be non negative." 377 | assert type(joints) is list, "Incorrect joints type. Should be list." 378 | assert len(joints) == 7, "Incorrect joints len. Should be 7." 379 | 380 | joint_trajectory_generator_msg_proto = JointTrajectoryGeneratorMessage(run_time=run_time, joints=joints) 381 | 382 | self.add_trajectory_params(joint_trajectory_generator_msg_proto.SerializeToString()) 383 | 384 | def add_goal_joint_velocities(self, run_time, joint_velocities, joint_accelerations): 385 | assert type(run_time) is float or type(run_time) is int,\ 386 | "Incorrect time type. Should be int or float." 387 | assert run_time >= 0, "Incorrect time. Should be non negative." 388 | assert type(joint_velocities) is list, "Incorrect joint_velocities type. Should be list." 389 | assert len(joint_velocities) == 7, "Incorrect joint_velocities len. Should be 7." 390 | assert type(joint_accelerations) is list, "Incorrect joint_accelerations type. Should be list." 391 | assert len(joint_accelerations) == 7, "Incorrect joint_accelerations len. Should be 7." 392 | 393 | joint_velocity_trajectory_generator_msg_proto = JointVelocityTrajectoryGeneratorMessage(run_time=run_time, 394 | joint_velocities=joint_velocities, joint_accelerations=joint_accelerations) 395 | 396 | self.add_trajectory_params(joint_velocity_trajectory_generator_msg_proto.SerializeToString()) 397 | 398 | def add_joint_dmp_params(self, run_time, joint_dmp_info, initial_sensor_values): 399 | assert type(run_time) is float or type(run_time) is int,\ 400 | "Incorrect run_time type. Should be int or float." 401 | assert run_time >= 0, "Incorrect run_time. Should be non negative." 402 | 403 | assert type(joint_dmp_info['tau']) is float or type(joint_dmp_info['tau']) is int,\ 404 | "Incorrect tau type. Should be int or float." 405 | assert joint_dmp_info['tau'] >= 0, "Incorrect tau. Should be non negative." 406 | 407 | assert type(joint_dmp_info['alpha']) is float or type(joint_dmp_info['alpha']) is int,\ 408 | "Incorrect alpha type. Should be int or float." 409 | assert joint_dmp_info['alpha'] >= 0, "Incorrect alpha. Should be non negative." 410 | 411 | assert type(joint_dmp_info['beta']) is float or type(joint_dmp_info['beta']) is int,\ 412 | "Incorrect beta type. Should be int or float." 413 | assert joint_dmp_info['beta'] >= 0, "Incorrect beta. Should be non negative." 414 | 415 | assert type(joint_dmp_info['num_basis']) is float or type(joint_dmp_info['num_basis']) is int,\ 416 | "Incorrect num basis type. Should be int or float." 417 | assert joint_dmp_info['num_basis'] >= 0, "Incorrect num basis. Should be non negative." 418 | 419 | assert type(joint_dmp_info['num_sensors']) is float or type(joint_dmp_info['num_sensors']) is int,\ 420 | "Incorrect num sensors type. Should be int or float." 421 | assert joint_dmp_info['num_sensors'] >= 0, "Incorrect num sensors. Should be non negative." 422 | 423 | assert type(joint_dmp_info['mu']) is list, "Incorrect basis mean type. Should be list." 424 | assert len(joint_dmp_info['mu']) == joint_dmp_info['num_basis'], \ 425 | "Incorrect basis mean len. Should be equal to num basis." 426 | 427 | assert type(joint_dmp_info['h']) is list, "Incorrect basis std dev type. Should be list." 428 | assert len(joint_dmp_info['h']) == joint_dmp_info['num_basis'], \ 429 | "Incorrect basis std dev len. Should be equal to num basis." 430 | 431 | assert type(initial_sensor_values) is list, "Incorrect initial sensor values type. Should be list." 432 | assert len(initial_sensor_values) == joint_dmp_info['num_sensors'], \ 433 | "Incorrect initial sensor values len. Should be equal to num sensors." 434 | 435 | weights = np.array(joint_dmp_info['weights']).reshape(-1).tolist() 436 | num_weights = 7 * int(joint_dmp_info['num_basis']) * int(joint_dmp_info['num_sensors']) 437 | 438 | assert len(weights) == num_weights, \ 439 | "Incorrect weights len. Should be equal to 7 * num basis * num sensors." 440 | 441 | assert self._skill_type == SkillType.ImpedanceControlSkill or \ 442 | self._skill_type == SkillType.JointPositionSkill, \ 443 | "Incorrect skill type. Should be ImpedanceControlSkill or JointPositionSkill." 444 | assert self._trajectory_generator_type == TrajectoryGeneratorType.JointDmpTrajectoryGenerator, \ 445 | "Incorrect trajectory generator type. Should be JointDmpTrajectoryGenerator" 446 | 447 | joint_dmp_trajectory_generator_msg_proto = JointDMPTrajectoryGeneratorMessage(run_time=run_time, 448 | tau=joint_dmp_info['tau'], alpha=joint_dmp_info['alpha'], beta=joint_dmp_info['beta'], 449 | num_basis=joint_dmp_info['num_basis'], num_sensor_values=joint_dmp_info['num_sensors'], 450 | basis_mean=joint_dmp_info['mu'], basis_std=joint_dmp_info['h'], 451 | weights=np.array(joint_dmp_info['weights']).reshape(-1).tolist(), 452 | initial_sensor_values=initial_sensor_values) 453 | 454 | self.add_trajectory_params(joint_dmp_trajectory_generator_msg_proto.SerializeToString()) 455 | 456 | 457 | def _check_dmp_info_parameters(self, dmp_info): 458 | assert type(dmp_info['tau']) in (float, int), "Incorrect tau type. Should be int or float." 459 | assert dmp_info['tau'] >= 0, "Incorrect tau. Should be non negative." 460 | 461 | assert type(dmp_info['alpha']) in (float, int), "Incorrect alpha type. Should be int or float." 462 | assert dmp_info['alpha'] >= 0, "Incorrect alpha. Should be non negative." 463 | 464 | assert type(dmp_info['beta']) in (float, int), "Incorrect beta type. Should be int or float." 465 | assert dmp_info['beta'] >= 0, "Incorrect beta. Should be non negative." 466 | 467 | assert type(dmp_info['num_basis']) in (float, int), "Incorrect num basis type. Should be int or float." 468 | assert dmp_info['num_basis'] >= 0, "Incorrect num basis. Should be non negative." 469 | 470 | assert type(dmp_info['num_sensors']) in (float, int), "Incorrect num sensors type. Should be int or float." 471 | assert dmp_info['num_sensors'] >= 0, "Incorrect num sensors. Should be non negative." 472 | 473 | assert type(dmp_info['mu']) is list, "Incorrect basis mean type. Should be list." 474 | assert len(dmp_info['mu']) == dmp_info['num_basis'], \ 475 | "Incorrect basis mean len. Should be equal to num basis." 476 | 477 | assert type(dmp_info['h']) is list, "Incorrect basis std dev type. Should be list." 478 | assert len(dmp_info['h']) == dmp_info['num_basis'], \ 479 | "Incorrect basis std dev len. Should be equal to num basis." 480 | 481 | def add_pose_dmp_params(self, orientation_only, position_only, ee_frame, run_time, pose_dmp_info, initial_sensor_values): 482 | assert type(run_time) is float or type(run_time) is int,\ 483 | "Incorrect run_time type. Should be int or float." 484 | assert run_time >= 0, "Incorrect run_time. Should be non negative." 485 | self._check_dmp_info_parameters(pose_dmp_info) 486 | assert type(initial_sensor_values) is list, "Incorrect initial sensor values type. Should be list." 487 | 488 | weights = np.array(pose_dmp_info['weights']).reshape(-1).tolist() 489 | 490 | if orientation_only or position_only: 491 | num_weights = 3 * int(pose_dmp_info['num_basis']) * int(pose_dmp_info['num_sensors']) 492 | assert len(weights) == num_weights, \ 493 | "Incorrect weights len. Should be equal to 3 * num basis * num sensors." 494 | assert len(initial_sensor_values) == 3 * pose_dmp_info['num_sensors'], \ 495 | "Incorrect initial sensor values len. Should be equal to 3 * num sensors." 496 | else: 497 | num_weights = 6 * int(pose_dmp_info['num_basis']) * int(pose_dmp_info['num_sensors']) 498 | assert len(weights) == num_weights, \ 499 | "Incorrect weights len. Should be equal to 6 * num basis * num sensors." 500 | assert len(initial_sensor_values) == 6 * pose_dmp_info['num_sensors'], \ 501 | "Incorrect initial sensor values len. Should be equal to 3 * num sensors." 502 | 503 | assert self._skill_type == SkillType.CartesianPoseSkill or \ 504 | self._skill_type == SkillType.ImpedanceControlSkill, \ 505 | "Incorrect skill type. Should be CartesianPoseSkill or ImpedanceControlSkill." 506 | assert self._trajectory_generator_type == TrajectoryGeneratorType.PoseDmpTrajectoryGenerator, \ 507 | "Incorrect trajectory generator type. Should be PoseDmpTrajectoryGenerator" 508 | 509 | pose_dmp_trajectory_generator_msg_proto = PoseDMPTrajectoryGeneratorMessage(orientation_only=orientation_only, 510 | position_only=position_only, ee_frame=ee_frame, run_time=run_time, 511 | tau=pose_dmp_info['tau'], alpha=pose_dmp_info['alpha'], beta=pose_dmp_info['beta'], 512 | num_basis=pose_dmp_info['num_basis'], num_sensor_values=pose_dmp_info['num_sensors'], 513 | basis_mean=pose_dmp_info['mu'], basis_std=pose_dmp_info['h'], 514 | weights=np.array(pose_dmp_info['weights']).reshape(-1).tolist(), 515 | initial_sensor_values=initial_sensor_values) 516 | 517 | self.add_trajectory_params(pose_dmp_trajectory_generator_msg_proto.SerializeToString()) 518 | 519 | 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): 520 | assert type(run_time) is float or type(run_time) is int,\ 521 | "Incorrect run_time type. Should be int or float." 522 | assert run_time >= 0, "Incorrect run_time. Should be non negative." 523 | assert len(goal_quat) == 4, "Invalid number of quaternion values for goal quaternion. Required: 4, provided: {}".format( 524 | len(goal_quat)) 525 | 526 | self._check_dmp_info_parameters(position_dmp_info) 527 | self._check_dmp_info_parameters(quat_dmp_info) 528 | assert type(initial_sensor_values) is list, "Incorrect initial sensor values type. Should be list." 529 | 530 | pos_weights = np.array(position_dmp_info['weights']).reshape(-1).tolist() 531 | num_pos_weights = 3 * int(position_dmp_info['num_basis']) * int(position_dmp_info['num_sensors']) 532 | assert len(pos_weights) == num_pos_weights, \ 533 | "Incorrect weights len. Should be equal to 3 * num basis * num sensors." 534 | 535 | quat_weights = np.array(quat_dmp_info['weights']).reshape(-1).tolist() 536 | num_quat_weights = 3 * int(quat_dmp_info['num_basis']) * int(quat_dmp_info['num_sensors']) 537 | assert len(quat_weights) == num_quat_weights, \ 538 | "Incorrect weights len. Should be equal to 4 * num basis * num sensors." 539 | 540 | assert self._skill_type == SkillType.CartesianPoseSkill or \ 541 | self._skill_type == SkillType.ImpedanceControlSkill, \ 542 | "Incorrect skill type. Should be CartesianPoseSkill or ImpedanceControlSkill." 543 | assert self._trajectory_generator_type == TrajectoryGeneratorType.QuaternionPoseDmpTrajectoryGenerator, \ 544 | "Incorrect trajectory generator type. Should be QuaternionPoseDmpTrajectoryGenerator" 545 | 546 | quat_pose_dmp_trajectory_generator_msg_proto = QuaternionPoseDMPTrajectoryGeneratorMessage(ee_frame=ee_frame, run_time=run_time, 547 | tau_pos=position_dmp_info['tau'], alpha_pos=position_dmp_info['alpha'], beta_pos=position_dmp_info['beta'], 548 | tau_quat=quat_dmp_info['tau'], alpha_quat=quat_dmp_info['alpha'], beta_quat=quat_dmp_info['beta'], 549 | num_basis_pos=position_dmp_info['num_basis'], num_sensor_values_pos=position_dmp_info['num_sensors'], 550 | num_basis_quat=quat_dmp_info['num_basis'], num_sensor_values_quat=quat_dmp_info['num_sensors'], 551 | pos_basis_mean=position_dmp_info['mu'], pos_basis_std=position_dmp_info['h'], 552 | quat_basis_mean=quat_dmp_info['mu'], quat_basis_std=quat_dmp_info['h'], 553 | pos_weights=np.array(position_dmp_info['weights']).reshape(-1).tolist(), 554 | quat_weights=np.array(quat_dmp_info['weights']).reshape(-1).tolist(), 555 | pos_initial_sensor_values=initial_sensor_values, 556 | quat_initial_sensor_values=initial_sensor_values, 557 | goal_time_quat=goal_quat_time, 558 | goal_quat_w=goal_quat[0], goal_quat_x=goal_quat[1], goal_quat_y=goal_quat[2], goal_quat_z=goal_quat[3]) 559 | 560 | self.add_trajectory_params(quat_pose_dmp_trajectory_generator_msg_proto.SerializeToString()) 561 | 562 | 563 | def add_run_time(self, run_time): 564 | assert type(run_time) is float or type(run_time) is int, \ 565 | "Incorrect run_time type. Should be int or float." 566 | assert run_time >= 0, "Incorrect run_time. Should be non negative." 567 | 568 | run_time_msg_proto = RunTimeMessage(run_time=run_time) 569 | self.add_trajectory_params(run_time_msg_proto.SerializeToString()) 570 | 571 | # Add checks for these 572 | def create_goal(self): 573 | goal = ExecuteSkillGoal() 574 | goal.skill_type = self._skill_type 575 | goal.skill_description = self._skill_desc 576 | goal.meta_skill_type = self._meta_skill_type 577 | goal.meta_skill_id = self._meta_skill_id 578 | goal.sensor_topics = self._sensor_topics 579 | goal.initial_sensor_values = self._initial_sensor_values 580 | goal.sensor_value_sizes = self._sensor_value_sizes 581 | goal.trajectory_generator_type = self._trajectory_generator_type 582 | goal.trajectory_generator_param_data = self._trajectory_generator_param_data 583 | goal.trajectory_generator_param_data_size = self._trajectory_generator_param_data_size 584 | goal.feedback_controller_type = self._feedback_controller_type 585 | goal.feedback_controller_param_data = self._feedback_controller_param_data 586 | goal.feedback_controller_param_data_size = \ 587 | self._feedback_controller_param_data_size 588 | goal.termination_handler_type = self._termination_handler_type 589 | goal.termination_handler_param_data = self._termination_handler_param_data 590 | goal.termination_handler_param_data_size = self._termination_handler_param_data_size 591 | goal.timer_type = self._timer_type 592 | goal.timer_params = self._timer_params 593 | goal.num_timer_params = self._num_timer_params 594 | return goal 595 | 596 | def feedback_callback(self, feedback): 597 | pass -------------------------------------------------------------------------------- /frankapy/utils.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from numba import jit 3 | from autolab_core import RigidTransform 4 | 5 | 6 | def franka_pose_to_rigid_transform(franka_pose, from_frame='franka_tool_base', to_frame='world'): 7 | np_franka_pose = np.array(franka_pose).reshape(4, 4).T 8 | pose = RigidTransform( 9 | rotation=np_franka_pose[:3, :3], 10 | translation=np_franka_pose[:3, 3], 11 | from_frame=from_frame, 12 | to_frame=to_frame 13 | ) 14 | return pose 15 | 16 | 17 | def convert_rigid_transform_to_array(rigid_tf): 18 | return rigid_tf.matrix.reshape(-1) 19 | 20 | 21 | def convert_array_to_rigid_transform(tf_array, from_frame='franka_tool', to_frame='world'): 22 | assert len(tf_array.shape) == 1 and tf_array.shape[0] == 16, "Invalid array shape" 23 | # Note that franka pose is in column format but this array is stored in normal 24 | # format hence we do not have do to any transposes. 25 | tf_mat = tf_array.reshape(4, 4) 26 | pose = RigidTransform( 27 | rotation=tf_mat[:3, :3], 28 | translation=tf_mat[:3, 3], 29 | from_frame=from_frame, 30 | to_frame=to_frame 31 | ) 32 | return pose 33 | 34 | 35 | @jit(nopython=True) 36 | def min_jerk_weight(t, T): 37 | r = t/T 38 | return (10 * r ** 3 - 15 * r ** 4 + 6 * r ** 5) 39 | 40 | 41 | @jit(nopython=True) 42 | def min_jerk(xi, xf, t, T): 43 | return xi + (xf - xi) * min_jerk_weight(t, T) 44 | 45 | 46 | @jit(nopython=True) 47 | def min_jerk_delta(xi, xf, t, T, dt): 48 | r = t/T 49 | return (xf - xi) * (30 * r ** 2 - 60 * r ** 3 + 30 * r ** 4) / T * dt 50 | 51 | 52 | def transform_to_list(T): 53 | return T.matrix.T.flatten().tolist() 54 | -------------------------------------------------------------------------------- /launch/franka_rviz.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /rviz/franka_basic_config.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Axes1 9 | Splitter Ratio: 0.5 10 | Tree Height: 775 11 | - Class: rviz/Selection 12 | Name: Selection 13 | - Class: rviz/Tool Properties 14 | Expanded: 15 | - /2D Pose Estimate1 16 | - /2D Nav Goal1 17 | - /Publish Point1 18 | Name: Tool Properties 19 | Splitter Ratio: 0.588679016 20 | - Class: rviz/Views 21 | Expanded: 22 | - /Current View1 23 | Name: Views 24 | Splitter Ratio: 0.5 25 | - Class: rviz/Time 26 | Experimental: false 27 | Name: Time 28 | SyncMode: 0 29 | SyncSource: "" 30 | Toolbars: 31 | toolButtonStyle: 2 32 | Visualization Manager: 33 | Class: "" 34 | Displays: 35 | - Alpha: 0.5 36 | Cell Size: 1 37 | Class: rviz/Grid 38 | Color: 160; 160; 164 39 | Enabled: true 40 | Line Style: 41 | Line Width: 0.0299999993 42 | Value: Lines 43 | Name: Grid 44 | Normal Cell Count: 0 45 | Offset: 46 | X: 0 47 | Y: 0 48 | Z: 0 49 | Plane: XY 50 | Plane Cell Count: 10 51 | Reference Frame: 52 | Value: true 53 | - Alpha: 1 54 | Class: rviz/RobotModel 55 | Collision Enabled: false 56 | Enabled: true 57 | Links: 58 | All Links Enabled: true 59 | Expand Joint Details: false 60 | Expand Link Details: false 61 | Expand Tree: false 62 | Link Tree Style: Links in Alphabetic Order 63 | panda_hand: 64 | Alpha: 1 65 | Show Axes: false 66 | Show Trail: false 67 | Value: true 68 | panda_leftfinger: 69 | Alpha: 1 70 | Show Axes: false 71 | Show Trail: false 72 | Value: true 73 | panda_link0: 74 | Alpha: 1 75 | Show Axes: false 76 | Show Trail: false 77 | Value: true 78 | panda_link1: 79 | Alpha: 1 80 | Show Axes: false 81 | Show Trail: false 82 | Value: true 83 | panda_link2: 84 | Alpha: 1 85 | Show Axes: false 86 | Show Trail: false 87 | Value: true 88 | panda_link3: 89 | Alpha: 1 90 | Show Axes: false 91 | Show Trail: false 92 | Value: true 93 | panda_link4: 94 | Alpha: 1 95 | Show Axes: false 96 | Show Trail: false 97 | Value: true 98 | panda_link5: 99 | Alpha: 1 100 | Show Axes: false 101 | Show Trail: false 102 | Value: true 103 | panda_link6: 104 | Alpha: 1 105 | Show Axes: false 106 | Show Trail: false 107 | Value: true 108 | panda_link7: 109 | Alpha: 1 110 | Show Axes: false 111 | Show Trail: false 112 | Value: true 113 | panda_link8: 114 | Alpha: 1 115 | Show Axes: false 116 | Show Trail: false 117 | panda_rightfinger: 118 | Alpha: 1 119 | Show Axes: false 120 | Show Trail: false 121 | Value: true 122 | Name: RobotModel 123 | Robot Description: robot_description 124 | TF Prefix: "" 125 | Update Interval: 0 126 | Value: true 127 | Visual Enabled: true 128 | - Class: rviz/TF 129 | Enabled: false 130 | Frame Timeout: 15 131 | Frames: 132 | All Enabled: true 133 | Marker Scale: 1 134 | Name: TF 135 | Show Arrows: true 136 | Show Axes: true 137 | Show Names: true 138 | Tree: 139 | {} 140 | Update Interval: 0 141 | Value: false 142 | - Class: rviz/Axes 143 | Enabled: true 144 | Length: 0.100000001 145 | Name: Axes 146 | Radius: 0.00999999978 147 | Reference Frame: panda_end_effector 148 | Value: true 149 | Enabled: true 150 | Global Options: 151 | Background Color: 48; 48; 48 152 | Default Light: true 153 | Fixed Frame: panda_link0 154 | Frame Rate: 30 155 | Name: root 156 | Tools: 157 | - Class: rviz/Interact 158 | Hide Inactive Objects: true 159 | - Class: rviz/MoveCamera 160 | - Class: rviz/Select 161 | - Class: rviz/FocusCamera 162 | - Class: rviz/Measure 163 | - Class: rviz/SetInitialPose 164 | Topic: /initialpose 165 | - Class: rviz/SetGoal 166 | Topic: /move_base_simple/goal 167 | - Class: rviz/PublishPoint 168 | Single click: true 169 | Topic: /clicked_point 170 | Value: true 171 | Views: 172 | Current: 173 | Class: rviz/Orbit 174 | Distance: 2.24045944 175 | Enable Stereo Rendering: 176 | Stereo Eye Separation: 0.0599999987 177 | Stereo Focal Distance: 1 178 | Swap Stereo Eyes: false 179 | Value: false 180 | Focal Point: 181 | X: 0 182 | Y: 0 183 | Z: 0 184 | Focal Shape Fixed Size: true 185 | Focal Shape Size: 0.0500000007 186 | Invert Z Axis: false 187 | Name: Current View 188 | Near Clip Distance: 0.00999999978 189 | Pitch: 0.485398054 190 | Target Frame: 191 | Value: Orbit (rviz) 192 | Yaw: 0.0903962478 193 | Saved: ~ 194 | Window Geometry: 195 | Displays: 196 | collapsed: false 197 | Height: 1056 198 | Hide Left Dock: false 199 | Hide Right Dock: false 200 | QMainWindow State: 000000ff00000000fd00000004000000000000016a00000396fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000396000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000396fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000002800000396000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073f0000003efc0100000002fb0000000800540069006d006501000000000000073f0000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000004ba0000039600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 201 | Selection: 202 | collapsed: false 203 | Time: 204 | collapsed: false 205 | Tool Properties: 206 | collapsed: false 207 | Views: 208 | collapsed: false 209 | Width: 1855 210 | X: 65 211 | Y: 24 212 | -------------------------------------------------------------------------------- /scripts/close_gripper.py: -------------------------------------------------------------------------------- 1 | from frankapy import FrankaArm 2 | 3 | 4 | if __name__ == '__main__': 5 | fa = FrankaArm() 6 | fa.close_gripper() -------------------------------------------------------------------------------- /scripts/open_gripper.py: -------------------------------------------------------------------------------- 1 | from frankapy import FrankaArm 2 | 3 | 4 | if __name__ == '__main__': 5 | fa = FrankaArm() 6 | fa.open_gripper() -------------------------------------------------------------------------------- /scripts/record_trajectory.py: -------------------------------------------------------------------------------- 1 | import argparse 2 | import time 3 | from frankapy import FrankaArm 4 | import pickle as pkl 5 | import numpy as np 6 | 7 | from frankapy.utils import convert_rigid_transform_to_array 8 | 9 | 10 | def create_formated_skill_dict(joints, end_effector_positions, time_since_skill_started): 11 | skill_dict = dict(skill_description='GuideMode', skill_state_dict=dict()) 12 | skill_dict['skill_state_dict']['q'] = np.array(joints) 13 | skill_dict['skill_state_dict']['O_T_EE'] = np.array(end_effector_positions) 14 | skill_dict['skill_state_dict']['time_since_skill_started'] = np.array(time_since_skill_started) 15 | 16 | # The key (0 here) usually represents the absolute time when the skill was started but 17 | formatted_dict = {0: skill_dict} 18 | return formatted_dict 19 | 20 | 21 | if __name__ == '__main__': 22 | parser = argparse.ArgumentParser() 23 | parser.add_argument('--time', '-t', type=float, default=10) 24 | parser.add_argument('--open_gripper', '-o', action='store_true') 25 | parser.add_argument('--file', '-f', default='franka_traj.pkl') 26 | args = parser.parse_args() 27 | 28 | print('Starting robot') 29 | fa = FrankaArm() 30 | if args.open_gripper: 31 | fa.open_gripper() 32 | 33 | print('Applying 0 force torque control for {}s'.format(args.time)) 34 | end_effector_position = [] 35 | joints = [] 36 | 37 | time_since_skill_started = [] 38 | fa.run_guide_mode(args.time, block=False) 39 | start_time = time.time() 40 | last_time = None 41 | 42 | while last_time is None or (last_time - start_time) < args.time: 43 | pose_array = convert_rigid_transform_to_array(fa.get_pose()) 44 | end_effector_position.append(pose_array) 45 | joints.append(fa.get_joints()) 46 | time_since_skill_started.append(time.time() - start_time) 47 | # add sleep to record at 100Hz 48 | time.sleep(0.0099) 49 | last_time = time.time() 50 | 51 | skill_dict = create_formated_skill_dict(joints, end_effector_position, time_since_skill_started) 52 | with open(args.file, 'wb') as pkl_f: 53 | pkl.dump(skill_dict, pkl_f) 54 | print("Did save skill dict: {}".format(args.file)) 55 | -------------------------------------------------------------------------------- /scripts/reset_arm.py: -------------------------------------------------------------------------------- 1 | import argparse 2 | from frankapy import FrankaArm 3 | 4 | if __name__ == '__main__': 5 | parser = argparse.ArgumentParser() 6 | parser.add_argument('--use_pose', '-u', action='store_true') 7 | parser.add_argument('--close_grippers', '-c', action='store_true') 8 | args = parser.parse_args() 9 | 10 | print('Starting robot') 11 | fa = FrankaArm() 12 | 13 | if args.use_pose: 14 | print('Reset with pose') 15 | fa.reset_pose() 16 | else: 17 | print('Reset with joints') 18 | fa.reset_joints() 19 | 20 | if args.close_grippers: 21 | print('Closing Grippers') 22 | fa.close_gripper() 23 | else: 24 | print('Opening Grippers') 25 | fa.open_gripper() -------------------------------------------------------------------------------- /scripts/run_guide_mode.py: -------------------------------------------------------------------------------- 1 | import argparse 2 | from frankapy import FrankaArm 3 | 4 | if __name__ == '__main__': 5 | parser = argparse.ArgumentParser() 6 | parser.add_argument('--time', '-t', type=float, default=100) 7 | parser.add_argument('--open_gripper', '-o', action='store_true') 8 | args = parser.parse_args() 9 | 10 | print('Starting robot') 11 | fa = FrankaArm() 12 | if args.open_gripper: 13 | fa.open_gripper() 14 | print('Applying 0 force torque control for {}s'.format(args.time)) 15 | fa.run_guide_mode(args.time) 16 | print(fa.get_pose()) 17 | print(fa.get_joints()) -------------------------------------------------------------------------------- /scripts/run_guide_mode_ee_frame.py: -------------------------------------------------------------------------------- 1 | import argparse 2 | from frankapy import FrankaArm 3 | 4 | if __name__ == '__main__': 5 | parser = argparse.ArgumentParser() 6 | parser.add_argument('--time', '-t', type=float, default=100) 7 | parser.add_argument('--open_gripper', '-o', action='store_true') 8 | args = parser.parse_args() 9 | 10 | print('Starting robot') 11 | fa = FrankaArm() 12 | if args.open_gripper: 13 | fa.open_gripper() 14 | fa.selective_guidance_mode(args.time, use_impedance=True, use_ee_frame=True, 15 | cartesian_impedances=[600.0, 0.0, 0.0, 0.0, 0.0, 0.0]) -------------------------------------------------------------------------------- /scripts/run_recorded_trajectory.py: -------------------------------------------------------------------------------- 1 | import argparse 2 | import pickle 3 | import numpy as np 4 | import time 5 | 6 | from autolab_core import RigidTransform 7 | from frankapy import FrankaArm, SensorDataMessageType 8 | from frankapy import FrankaConstants as FC 9 | from frankapy.proto_utils import sensor_proto2ros_msg, make_sensor_group_msg 10 | from frankapy.proto import PosePositionSensorMessage, ShouldTerminateSensorMessage 11 | from franka_interface_msgs.msg import SensorDataGroup 12 | 13 | from frankapy.utils import convert_array_to_rigid_transform 14 | 15 | import rospy 16 | 17 | 18 | if __name__ == "__main__": 19 | parser = argparse.ArgumentParser() 20 | parser.add_argument('--trajectory_pickle', '-t', type=str, required=True, 21 | help='Path to trajectory (in pickle format) to replay.') 22 | parser.add_argument('--open_gripper', '-o', action='store_true') 23 | args = parser.parse_args() 24 | 25 | print('Starting robot') 26 | fa = FrankaArm() 27 | fa.reset_joints() 28 | 29 | rospy.loginfo('Loading Trajectory') 30 | 31 | with open(args.trajectory_pickle, 'rb') as pkl_f: 32 | skill_data = pickle.load(pkl_f) 33 | 34 | assert skill_data[0]['skill_description'] == 'GuideMode', \ 35 | "Trajectory not collected in guide mode" 36 | skill_state_dict = skill_data[0]['skill_state_dict'] 37 | 38 | T = float(skill_state_dict['time_since_skill_started'][-1]) 39 | dt = 0.01 40 | ts = np.arange(0, T, dt) 41 | 42 | pose_traj = skill_state_dict['O_T_EE'] 43 | # Goto the first position in the trajectory. 44 | print(convert_array_to_rigid_transform(pose_traj[0]).matrix) 45 | fa.goto_pose(convert_array_to_rigid_transform(pose_traj[0]), 46 | duration=4.0, 47 | cartesian_impedances=[600.0, 600.0, 600.0, 50.0, 50.0, 50.0]) 48 | 49 | rospy.loginfo('Initializing Sensor Publisher') 50 | pub = rospy.Publisher(FC.DEFAULT_SENSOR_PUBLISHER_TOPIC, SensorDataGroup, queue_size=10) 51 | rate = rospy.Rate(1 / dt) 52 | 53 | rospy.loginfo('Publishing pose trajectory...') 54 | # To ensure skill doesn't end before completing trajectory, make the buffer time much longer than needed 55 | fa.goto_pose(convert_array_to_rigid_transform(pose_traj[1]), 56 | duration=T, 57 | dynamic=True, 58 | buffer_time=10, 59 | cartesian_impedances=[600.0, 600.0, 600.0, 50.0, 50.0, 50.0] 60 | ) 61 | init_time = rospy.Time.now().to_time() 62 | for i in range(2, len(ts)): 63 | timestamp = rospy.Time.now().to_time() - init_time 64 | pose_tf = convert_array_to_rigid_transform(pose_traj[i]) 65 | traj_gen_proto_msg = PosePositionSensorMessage( 66 | id=i, 67 | timestamp=timestamp, 68 | position=pose_tf.translation, 69 | quaternion=pose_tf.quaternion 70 | ) 71 | ros_msg = make_sensor_group_msg( 72 | trajectory_generator_sensor_msg=sensor_proto2ros_msg( 73 | traj_gen_proto_msg, 74 | SensorDataMessageType.POSE_POSITION), 75 | ) 76 | 77 | # Sleep the same amount as the trajectory was recorded in 78 | dt = skill_state_dict['time_since_skill_started'][i] - skill_state_dict['time_since_skill_started'][i-1] 79 | rospy.loginfo('Publishing: ID {}, dt: {:.4f}'.format(traj_gen_proto_msg.id, dt)) 80 | pub.publish(ros_msg) 81 | time.sleep(dt) 82 | # Finished trajectory 83 | if i >= pose_traj.shape[0] - 1: 84 | break 85 | 86 | # Stop the skill 87 | # Alternatively can call fa.stop_skill() 88 | term_proto_msg = ShouldTerminateSensorMessage(timestamp=rospy.Time.now().to_time() - init_time, 89 | should_terminate=True) 90 | ros_msg = make_sensor_group_msg( 91 | termination_handler_sensor_msg=sensor_proto2ros_msg( 92 | term_proto_msg, SensorDataMessageType.SHOULD_TERMINATE) 93 | ) 94 | pub.publish(ros_msg) 95 | 96 | rospy.loginfo('Done') 97 | -------------------------------------------------------------------------------- /scripts/test_virtual_walls.py: -------------------------------------------------------------------------------- 1 | from frankapy import FrankaArm 2 | 3 | if __name__ == '__main__': 4 | print('Starting robot') 5 | fa = FrankaArm() 6 | 7 | fa.reset_joints() 8 | 9 | pose = fa.get_pose() 10 | pose.translation[0] = 0.75 11 | 12 | # This should trigger an error 13 | fa.goto_pose(pose) 14 | -------------------------------------------------------------------------------- /setup.py: -------------------------------------------------------------------------------- 1 | """ 2 | FrankaPy Franka Panda Robot Control Library 3 | """ 4 | from setuptools import setup 5 | 6 | requirements = [ 7 | 'autolab_core', 8 | 'empy==3.3.4', 9 | 'numpy', 10 | 'numpy-quaternion', 11 | 'numba', 12 | 'rospkg', 13 | 'catkin-tools', 14 | 'protobuf' 15 | ] 16 | 17 | setup(name='frankapy', 18 | version='1.0.0', 19 | description='FrankaPy Franka Panda Robot Control Library', 20 | author='Kevin Zhang, Mohit Sharma, Jacky Liang, Oliver Kroemer', 21 | author_email='', 22 | package_dir = {'': '.'}, 23 | packages=['frankapy'], 24 | install_requires = requirements, 25 | extras_require = {} 26 | ) 27 | -------------------------------------------------------------------------------- /urdf/hand.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /urdf/hand.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | -------------------------------------------------------------------------------- /urdf/panda_arm.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /urdf/panda_arm.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | 140 | 141 | 142 | 143 | 144 | 145 | 146 | 147 | 148 | 149 | 150 | 151 | 152 | 153 | 154 | 155 | 156 | 157 | 158 | 159 | 160 | 161 | 162 | 163 | 164 | 165 | 166 | 167 | 168 | 169 | 170 | 171 | 172 | -------------------------------------------------------------------------------- /urdf/panda_arm_hand.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | --------------------------------------------------------------------------------