Showing preview only (295K chars total). Download the full file or copy to clipboard to get everything.
Repository: iamlab-cmu/frankapy
Branch: master
Commit: 4ba2872ddfa4
Files: 75
Total size: 275.5 KB
Directory structure:
gitextract_4vicwimb/
├── .github/
│ └── workflows/
│ └── docs_pages_workflow.yml
├── .gitignore
├── .gitmodules
├── LICENSE
├── README.md
├── bash_scripts/
│ ├── make_catkin.sh
│ ├── make_proto.sh
│ ├── start_control_pc.sh
│ ├── start_franka_gripper_on_control_pc.sh
│ ├── start_franka_interface_on_control_pc.sh
│ ├── start_franka_ros_interface_on_control_pc.sh
│ └── start_rosmaster.sh
├── cfg/
│ └── april_tag_pick_place_cfg.yaml
├── docs/
│ ├── .gitignore
│ ├── Makefile
│ ├── buildDocs.sh
│ ├── conf.py
│ ├── frankaarm.rst
│ ├── index.rst
│ ├── install.rst
│ ├── make.bat
│ ├── network.rst
│ ├── running.rst
│ └── support.rst
├── examples/
│ ├── T_RSD415_franka.tf
│ ├── T_RS_mount_delta.tf
│ ├── april_tag_pick_place.py
│ ├── example_movements.py
│ ├── execute_joint_torques.py
│ ├── go_to_joints_with_joint_impedances.py
│ ├── move_robot.py
│ ├── record_trajectory.py
│ ├── run_dynamic_cartesian_velocities.py
│ ├── run_dynamic_joint_velocities.py
│ ├── run_dynamic_joints.py
│ ├── run_dynamic_pose.py
│ ├── run_hfpc.py
│ ├── run_pose_dmp.py
│ ├── run_quaternion_pose_dmp.py
│ └── run_recorded_trajectory.py
├── frankapy/
│ ├── __init__.py
│ ├── exceptions.py
│ ├── franka_arm.py
│ ├── franka_arm_state_client.py
│ ├── franka_constants.py
│ ├── franka_interface_common_definitions.py
│ ├── proto/
│ │ ├── __init__.py
│ │ ├── feedback_controller_params_msg.proto
│ │ ├── feedback_controller_params_msg_pb2.py
│ │ ├── sensor_msg.proto
│ │ ├── sensor_msg_pb2.py
│ │ ├── termination_handler_params_msg.proto
│ │ ├── termination_handler_params_msg_pb2.py
│ │ ├── trajectory_generator_params_msg.proto
│ │ └── trajectory_generator_params_msg_pb2.py
│ ├── proto_utils.py
│ ├── ros_utils.py
│ ├── skill_list.py
│ └── utils.py
├── launch/
│ └── franka_rviz.launch
├── rviz/
│ └── franka_basic_config.rviz
├── scripts/
│ ├── close_gripper.py
│ ├── open_gripper.py
│ ├── record_trajectory.py
│ ├── reset_arm.py
│ ├── run_guide_mode.py
│ ├── run_guide_mode_ee_frame.py
│ ├── run_recorded_trajectory.py
│ └── test_virtual_walls.py
├── setup.py
└── urdf/
├── hand.urdf.xacro
├── hand.xacro
├── panda_arm.urdf.xacro
├── panda_arm.xacro
└── panda_arm_hand.urdf.xacro
================================================
FILE CONTENTS
================================================
================================================
FILE: .github/workflows/docs_pages_workflow.yml
================================================
name: docs_pages_workflow
# execute this workflow automatically when a we push to master
on:
push:
branches: [ master ]
jobs:
build_docs_job:
runs-on: ubuntu-latest
container: ubuntu:focal
steps:
- name: Prereqs
env:
GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }}
run: |
apt-get update
apt-get install -y git
git clone --depth 1 "https://token:${GITHUB_TOKEN}@github.com/${GITHUB_REPOSITORY}.git" .
shell: bash
- name: Setup ROS environment
uses: ros-tooling/setup-ros@0.2.1
with:
required-ros-distributions: noetic
- name: Execute script to build our documentation and update pages
env:
GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }}
run: "docs/buildDocs.sh"
shell: bash
================================================
FILE: .gitignore
================================================
# Byte-compiled / optimized / DLL files
__pycache__/
*.py[cod]
*$py.class
# C extensions
*.so
# Distribution / packaging
.Python
build/
develop-eggs/
dist/
downloads/
eggs/
.eggs/
lib/
lib64/
parts/
sdist/
var/
wheels/
pip-wheel-metadata/
share/python-wheels/
*.egg-info/
.installed.cfg
*.egg
MANIFEST
# PyInstaller
# Usually these files are written by a python script from a template
# before PyInstaller builds the exe, so as to inject date/other infos into it.
*.manifest
*.spec
# Installer logs
pip-log.txt
pip-delete-this-directory.txt
# Unit test / coverage reports
htmlcov/
.tox/
.nox/
.coverage
.coverage.*
.cache
nosetests.xml
coverage.xml
*.cover
*.py,cover
.hypothesis/
.pytest_cache/
# Translations
*.mo
*.pot
# Django stuff:
*.log
local_settings.py
db.sqlite3
db.sqlite3-journal
# Flask stuff:
instance/
.webassets-cache
# Scrapy stuff:
.scrapy
# PyBuilder
target/
# Jupyter Notebook
.ipynb_checkpoints
# IPython
profile_default/
ipython_config.py
# pyenv
.python-version
# pipenv
# According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control.
# However, in case of collaboration, if having platform-specific dependencies or dependencies
# having no cross-platform support, pipenv may install dependencies that don't work, or not
# install all needed dependencies.
#Pipfile.lock
# PEP 582; used by e.g. github.com/David-OConnor/pyflow
__pypackages__/
# Celery stuff
celerybeat-schedule
celerybeat.pid
# SageMath parsed files
*.sage.py
# Environments
.env
.venv
env/
venv/
ENV/
env.bak/
venv.bak/
# Spyder project settings
.spyderproject
.spyproject
# Rope project settings
.ropeproject
# mkdocs documentation
/site
# mypy
.mypy_cache/
.dmypy.json
dmypy.json
# Pyre type checker
.pyre/
# ROS
catkin_ws/devel
catkin_ws/build
catkin_ws/logs
catkin_ws/.catkin_tools
================================================
FILE: .gitmodules
================================================
[submodule "catkin_ws/src/franka-interface-msgs"]
path = catkin_ws/src/franka-interface-msgs
url = https://github.com/iamlab-cmu/franka-interface-msgs.git
================================================
FILE: LICENSE
================================================
MIT License
Copyright (c) 2021 Carnegie Mellon University
Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:
The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
SOFTWARE.
================================================
FILE: README.md
================================================
# frankapy
This is a software package used for controlling and learning skills on the Franka Emika Panda Research Robot Arm.
Installation Instructions and Network Configuration Instructions are also available here: [https://iamlab-cmu.github.io/frankapy](https://iamlab-cmu.github.io/frankapy)
To join the Discord community, click the link [here](https://discord.gg/r6r7dttMwZ).
## Requirements
* A computer with the Ubuntu 18.04 / 20.04.
* ROS Melodic / Noetic
* [Protocol Buffers](https://github.com/protocolbuffers/protobuf)
## Computer Setup Instructions
This library is intended to be installed on any computer in the same ROS network with the computer that interfaces with the Franka (we call the latter the Control PC).
`FrankaPy` will send commands to [franka-interface](https://github.com/iamlab-cmu/franka-interface), which actually controls the robot.
## Install ProtoBuf
**This is only needed if you plan to modify the proto messages. You don't need to install or compile protobuf for using frankapy**
We use both C++ and Python versions of protobufs so you would need to install Protobufs from source.
Do `nproc` to find out how many cores you have, and use that as the `N` number in the `make` command below:
```shell
sudo apt-get install autoconf automake libtool curl make g++ unzip
wget https://github.com/protocolbuffers/protobuf/releases/download/v21.8/protobuf-all-21.8.zip
unzip protobuf-all-21.8.zip
cd protobuf-21.8
./configure
make -jN
make check -jN
sudo make install
sudo ldconfig
```
See detailed instructions [here](https://github.com/protocolbuffers/protobuf/blob/master/src/README.md)
## Installation
1. Clone Repo and its Submodules:
```bash
git clone --recurse-submodules git@github.com:iamlab-cmu/frankapy.git
```
All directories below are given relative to `/frankapy`.
2. First source into your virtualenv or conda env (should be Python 3.6). Then:
```bash
pip install -e .
```
3. To compile the catkin_ws use the following script:
```bash
./bash_scripts/make_catkin.sh
```
4. To allow asynchronous gripper commands, we use the franka\_ros package, so install libfranka and franka\_ros using the following command (Change melodic to noetic if you are on Ubuntu 20.04:
```bash
sudo apt install ros-melodic-libfranka ros-melodic-franka-ros
```
5. To make the protobufs use the following script (**you don't need to do this if you haven't modified the proto messages**):
```bash
./bash_scripts/make_proto.sh
```
## Configuring the network with the Control PC
### Ethernet
1. If you have an ethernet cable directly between the Control PC and the one sending Frankapy commands, you can go into the Ubuntu Network Connections Menu on the Control PC.
2. Select the Ethernet connection that corresponds to the port that you plugged the ethernet cable into and then click edit.
3. Go to the IPv4 Settings Tab and switch from Automatic (DHCP) to Manual.
4. Add a static ip address like 192.168.1.3 on the Control PC with netmask 24 and then click save.
5. Then do the same on the FrankaPy PC but instead set the static ip address to be 192.168.1.2.
### Wifi
1. If you are only communicating with the Control PC over Wifi, use the command `ifconfig` in order to get the wifi ip address of both computers and note them down.
### Editing the /etc/hosts file
1. Now that you have the ip addresses for both the Control PC and FrankaPy PC, you will need to edit the /etc/hosts files on both in order to allow communication between the 2 over ROS.
2. On the Control PC, run the command: `sudo gedit /etc/hosts`
3. If you are using an Ethernet connection, then add the following above the line `# The following lines are desirable for IPv6 capable hosts`:
```bash
192.168.1.2 [frankapy-pc-name]
```
Otherwise substitute 192.168.1.2 with the ip address of the FrankaPy PC that you discovered using the command `ifconfig`.
4. Afterwards, on the FrankaPy PC, again run the command `sudo gedit /etc/hosts` and add the line:
```bash
192.168.1.3 [control-pc-name]
```
Otherwise substitute 192.168.1.3 with the ip address of the Control PC that you discovered using the command `ifconfig`.
5. Now you should be able to ssh between the FrankaPy PC and the Control PC using the command:
```bash
ssh [control-pc-username]@[control-pc-name]
Input password to control-pc.
```
## Setting Up SSH Key to Control PC
1. Generate an ssh key by executing the following commands or reading the instructions here: https://help.github.com/en/articles/generating-a-new-ssh-key-and-adding-it-to-the-ssh-agent
```bash
ssh-keygen -t rsa -b 4096 -C "your_email@example.com"
[Press enter]
[Press enter]
[Press enter]
eval "$(ssh-agent -s)"
ssh-add ~/.ssh/id_rsa
```
2. Upload your public ssh key to the control pc.
1. In a new terminal, ssh to the control PC.
```bash
ssh [control-pc-username]@[control-pc-name]
Input password to control-pc.
```
2. Use your favorite text editor to open the authorized_keys file.
```bash
vim ~/.ssh/authorized_keys
```
3. In a separate terminal on your Workhorse PC, use your favorite text editor to open your id_rsa.pub file.
```bash
vim ~/.ssh/id_rsa.pub
```
4. Copy the contents from your id_rsa.pub file to a new line on the authorized_keys file on the Control PC. Then save.
5. Open a new terminal and try sshing to the control PC and it should no longer require a password.
3. (Optional) Upload your ssh key to github by following instructions here: https://help.github.com/en/articles/adding-a-new-ssh-key-to-your-github-account
## Unlocking the Franka Robot
1. In a new terminal, ssh to the control PC with option -X.
```bash
ssh -X [control-pc-username]@[control-pc-name]
```
2. Open a web browser, either firefox or google chrome.
```bash
firefox
```
3. Go to 172.16.0.2 in the web browser.
4. (Optional) Input the username admin and the password to login to the Franka Desk GUI.
5. Unlock the robot by clicking the unlock button on the bottom right of the web interface.
6. If the robot has pink lights, press down on the e-stop and then release it and the robot should turn blue. If the robot is white, just release the e-stop and it should also turn blue.
## Running the Franka Robot
1. Make sure that both the user stop and the brakes of the Franka robot have been unlocked in the Franka Desk GUI.
2. Open up a new terminal and go to the frankapy directory.
```bash
bash ./bash_scripts/start_control_pc.sh -i [control-pc-name]
```
Please see the `start_control_pc.sh` bash script for additional arguments, including specifying a custom directory for where `franka-interface` is installed on the Control PC as well as the username of the account on the Control PC. By default the username is `iam-lab`.
3. Open up a new terminal and go to the frankapy directory. Do:
```bash
source catkin_ws/devel/setup.bash
```
Be in the same virtualenv or Conda env that FrankaPy was installed in. Place your hand on top of the e-stop. Reset the robot pose with the following command.
```bash
python scripts/reset_arm.py
```
See example scripts in the `examples/` and `scripts/` to learn how to use the `FrankaPy` python package.
## Citation
If this library proves useful to your research, please cite the paper below::
```
@article{zhang2020modular,
title={A modular robotic arm control stack for research: Franka-interface and frankapy},
author={Zhang, Kevin and Sharma, Mohit and Liang, Jacky and Kroemer, Oliver},
journal={arXiv preprint arXiv:2011.02398},
year={2020}
}
```
================================================
FILE: bash_scripts/make_catkin.sh
================================================
cd catkin_ws
catkin build
cd ..
================================================
FILE: bash_scripts/make_proto.sh
================================================
protoc -I=frankapy/proto/ --python_out=frankapy/proto/ frankapy/proto/*.proto
================================================
FILE: bash_scripts/start_control_pc.sh
================================================
#!/bin/bash
die () {
echo >&2 "$@"
exit 1
}
usage="$(basename "$0") [-h] [-i xxx.xxx.xxx.xxx] -- Start control PC from workstation
where:
-h show this help text
-i IP address for the Control PC. If you use localhost, it will treat the current PC as the Control PC.
-u Username on control PC. (default iam-lab)
-p Control PC password
-d Path to franka_interface on Control PC (default Documents/franka-interface)
-r Robot number (default 1)
-a Robot IP address (default 172.16.0.2)
-s Start franka-interface on Control PC (0 / 1 (default))
-g Robot has gripper (0 / 1 (default))
-o Using old gripper commands (0 (default) / 1)
-l Log at 1kHz on franka-interface (0 (default) / 1)
-e Stop franka-interface when an error has occurred (0 (default) / 1)
./start_control_pc.sh -i iam-space
./start_control_pc.sh -i iam-space -u iam-lab -p 12345678 -d ~/Documents/franka-interface -r 1 -s 0
"
control_pc_uname="iam-lab"
control_pc_use_password=0
control_pc_password=""
control_pc_franka_interface_path="Documents/franka-interface"
start_franka_interface=1
robot_number=1
robot_ip="172.16.0.2"
with_gripper=1
old_gripper=0
log_on_franka_interface=0
stop_on_error=0
while getopts ':h:i:u:p:d:r:a:s:g:o:l:e' option; do
case "${option}" in
h) echo "$usage"
exit
;;
i) control_pc_ip_address=$OPTARG
;;
u) control_pc_uname=$OPTARG
;;
p) control_pc_password=$OPTARG
control_pc_use_password=1
;;
d) control_pc_franka_interface_path=$OPTARG
;;
r) robot_number=$OPTARG
;;
a) robot_ip=$OPTARG
;;
s) start_franka_interface=$OPTARG
;;
g) with_gripper=$OPTARG
;;
o) old_gripper=$OPTARG
;;
l) log_on_franka_interface=$OPTARG
;;
e) stop_on_error=$OPTARG
;;
:) printf "missing argument for -%s\n" "$OPTARG" >&2
echo "$usage" >&2
exit 1
;;
\?) printf "illegal option: -%s\n" "$OPTARG" >&2
echo "$usage" >&2
exit 1
;;
esac
done
shift $((OPTIND - 1))
workstation_ip_address="`hostname`"
# Notify the IP addresses being used.
echo "Control PC IP uname/address: "$control_pc_uname"@"$control_pc_ip_address
echo "Workstation IP address: "$workstation_ip_address
if [ "$control_pc_use_password" -eq 0 ]; then
echo "Will not use password to ssh into control pc."
else
echo "Will use input password to ssh into control pc."
fi
DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )"
# Start rosmaster in a new gnome-terminal if not already running
if ! pgrep -x "roscore" > /dev/null
then
start_rosmaster_path="$DIR/start_rosmaster.sh"
echo "Will start ROS master in new terminal."$start_rosmaster_path
gnome-terminal --working-directory="$DIR" -- bash $start_rosmaster_path
sleep 3
echo "Did start ROS master in new terminal."
else
echo "Roscore is already running"
fi
if [ "$with_gripper" -eq 0 ]; then
let old_gripper=0
fi
if [ "$start_franka_interface" -eq 1 ]; then
# ssh to the control pc and start franka_interface in a new gnome-terminal
start_franka_interface_on_control_pc_path="$DIR/start_franka_interface_on_control_pc.sh"
echo "Will ssh to control PC and start franka-interface."
gnome-terminal --working-directory="$DIR" -- bash $start_franka_interface_on_control_pc_path $robot_ip $old_gripper $log_on_franka_interface $stop_on_error $control_pc_uname $control_pc_ip_address $control_pc_franka_interface_path $control_pc_use_password $control_pc_password
echo "Done"
sleep 3
else
echo "Will not start franka-interface on the control pc."
fi
# ssh to the control pc and start ROS action server in a new gnome-terminal
start_franka_ros_interface_on_control_pc_path="$DIR/start_franka_ros_interface_on_control_pc.sh"
echo "Will ssh to control PC and start ROS action server."
gnome-terminal --working-directory="$DIR" -- bash $start_franka_ros_interface_on_control_pc_path $control_pc_uname $control_pc_ip_address $workstation_ip_address $control_pc_franka_interface_path $robot_number $control_pc_use_password $control_pc_password
sleep 3
if [ "$with_gripper" -eq 1 ] && [ "$old_gripper" -eq 0 ]; then
start_franka_gripper_on_control_pc_path="$DIR/start_franka_gripper_on_control_pc.sh"
echo "Will ssh to control PC and start ROS action server."
gnome-terminal --working-directory="$DIR" -- bash $start_franka_gripper_on_control_pc_path $control_pc_uname $control_pc_ip_address $workstation_ip_address $control_pc_franka_interface_path $robot_number $robot_ip $control_pc_use_password $control_pc_password
sleep 3
else
echo "Will not start franka gripper on the control pc."
fi
echo "Done"
================================================
FILE: bash_scripts/start_franka_gripper_on_control_pc.sh
================================================
#!/bin/bash
control_pc_uname=${1}
control_pc_ip_address=${2}
workstation_ip_address=${3}
control_pc_franka_interface_path=${4}
robot_number=${5}
robot_ip=${6}
control_pc_use_password=${7}
control_pc_password=${8}
rosmaster_path="bash_scripts/set_rosmaster.sh"
catkin_ws_setup_path="catkin_ws/devel/setup.bash"
if [ "$control_pc_ip_address" = "localhost" ]; then
cd $HOME
cd $control_pc_franka_interface_path
source $catkin_ws_setup_path
roslaunch franka_ros_interface franka_gripper.launch robot_num:=$robot_number robot_ip:=$robot_ip
bash
else
if [ "$control_pc_use_password" = "0" ]; then
ssh -tt $control_pc_uname@$control_pc_ip_address << EOSSH
cd $control_pc_franka_interface_path
source $rosmaster_path $control_pc_ip_address $workstation_ip_address
source $catkin_ws_setup_path
roslaunch franka_ros_interface franka_gripper.launch robot_num:=$robot_number robot_ip:=$robot_ip
bash
EOSSH
else
sshpass -p "$control_pc_password" ssh -tt -o StrictHostKeyChecking=no $control_pc_uname@$control_pc_ip_address << EOSSH
cd $control_pc_franka_interface_path
source $rosmaster_path $control_pc_ip_address $workstation_ip_address
source $catkin_ws_setup_path
roslaunch franka_ros_interface franka_gripper.launch robot_num:=$robot_number robot_ip:=$robot_ip
bash
EOSSH
fi
fi
================================================
FILE: bash_scripts/start_franka_interface_on_control_pc.sh
================================================
#!/bin/bash
robot_ip=${1}
with_gripper=${2}
log_on_franka_interface=${3}
stop_on_error=${4}
control_pc_uname=${5}
control_pc_ip_address=${6}
control_pc_franka_interface_path=${7}
control_pc_use_password=${8}
control_pc_password=${9}
franka_interface_stop_on_error=${10}
if [ "$control_pc_ip_address" = "localhost" ]; then
cd $HOME
cd $control_pc_franka_interface_path
cd build
./franka_interface --robot_ip $robot_ip --with_gripper $with_gripper --log $log_on_franka_interface --stop_on_error $stop_on_error
bash
else
if [ "$control_pc_use_password" = "0" ]; then
ssh -tt $control_pc_uname@$control_pc_ip_address << EOSSH
cd $control_pc_franka_interface_path
cd build
./franka_interface --robot_ip $robot_ip --with_gripper $with_gripper --log $log_on_franka_interface --stop_on_error $stop_on_error
bash
EOSSH
else
sshpass -p "$control_pc_password" ssh -tt -o StrictHostKeyChecking=no $control_pc_uname@$control_pc_ip_address << EOSSH
cd $control_pc_franka_interface_path
cd build
echo $stop_on_error
./franka_interface --robot_ip $robot_ip --with_gripper $with_gripper --log $log_on_franka_interface --stop_on_error $stop_on_error
bash
EOSSH
fi
fi
================================================
FILE: bash_scripts/start_franka_ros_interface_on_control_pc.sh
================================================
#!/bin/bash
control_pc_uname=${1}
control_pc_ip_address=${2}
workstation_ip_address=${3}
control_pc_franka_interface_path=${4}
robot_number=${5}
control_pc_use_password=${6}
control_pc_password=${7}
rosmaster_path="bash_scripts/set_rosmaster.sh"
catkin_ws_setup_path="catkin_ws/devel/setup.bash"
if [ "$control_pc_ip_address" = "localhost" ]; then
cd $HOME
cd $control_pc_franka_interface_path
source $catkin_ws_setup_path
roslaunch franka_ros_interface franka_ros_interface.launch robot_num:=$robot_number
bash
else
if [ "$control_pc_use_password" = "0" ]; then
ssh -tt $control_pc_uname@$control_pc_ip_address << EOSSH
cd $control_pc_franka_interface_path
source $rosmaster_path $control_pc_ip_address $workstation_ip_address
source $catkin_ws_setup_path
roslaunch franka_ros_interface franka_ros_interface.launch robot_num:=$robot_number
bash
EOSSH
else
sshpass -p "$control_pc_password" ssh -tt -o StrictHostKeyChecking=no $control_pc_uname@$control_pc_ip_address << EOSSH
cd $control_pc_franka_interface_path
source $rosmaster_path $control_pc_ip_address $workstation_ip_address
source $catkin_ws_setup_path
roslaunch franka_ros_interface franka_ros_interface.launch robot_num:=$robot_number
bash
EOSSH
fi
fi
================================================
FILE: bash_scripts/start_rosmaster.sh
================================================
#!/bin/bash
roscore
================================================
FILE: cfg/april_tag_pick_place_cfg.yaml
================================================
rs:
id: 0
frame: realsense
filter_depth: True
april_tag:
detector:
families: tag36h11
border: 1
nthreads: 4
quad_decimate: 1.0
quad_blur: 0.0
refine_edges: True
refine_decode: False
refine_pose: False
debug: False
quad_contours: True
tag_size: 0.0254 # 1 inch in meters
T_camera_ee_path: examples/T_RSD415_franka.tf
T_camera_mount_path: examples/T_RS_mount_delta.tf
cube_size: 0.04
vis_detect: True
================================================
FILE: docs/.gitignore
================================================
*.swp
/_build
/doctrees
================================================
FILE: docs/Makefile
================================================
# Minimal makefile for Sphinx documentation
#
# You can set these variables from the command line, and also
# from the environment for the first two.
SPHINXOPTS ?=
SPHINXBUILD ?= sphinx-build
SOURCEDIR = .
BUILDDIR = _build
# Put it first so that "make" without argument is like "make help".
help:
@$(SPHINXBUILD) -M help "$(SOURCEDIR)" "$(BUILDDIR)" $(SPHINXOPTS) $(O)
.PHONY: help Makefile
# Catch-all target: route all unknown targets to Sphinx using the new
# "make mode" option. $(O) is meant as a shortcut for $(SPHINXOPTS).
%: Makefile
@$(SPHINXBUILD) -M $@ "$(SOURCEDIR)" "$(BUILDDIR)" $(SPHINXOPTS) $(O)
================================================
FILE: docs/buildDocs.sh
================================================
#!/bin/bash
set -x
################################################################################
# File: buildDocs.sh
# Purpose: Script that builds our documentation using sphinx and updates GitHub
# Pages. This script is executed by:
# .github/workflows/docs_pages_workflow.yml
#
# Authors: Michael Altfield <michael@michaelaltfield.net>
# Created: 2020-07-17
# Updated: 2020-07-17
# Version: 0.1
################################################################################
###################
# INSTALL DEPENDS #
###################
apt-get update
apt-get -y install git rsync python3-sphinx python3-sphinx-rtd-theme python3-setuptools python3-pip
apt-get -y install ros-noetic-libfranka ros-noetic-franka-ros
sudo rosdep init
rosdep update
sudo -H pip3 install --upgrade pip
sudo -H pip3 install --upgrade packaging>=24 ordered-set>=3.1.1 more_itertools>=8.8 jaraco.text>=3.7 importlib_resources>=5.10.2 importlib_metadata>=6 tomli>=2.0.1 wheel>=0.43.0 platformdirs>=2.6.2
sudo -H pip3 install .
source /opt/ros/noetic/setup.bash
git config --global --add safe.directory /__w/frankapy/frankapy
git submodule update --init --recursive
cd catkin_ws
catkin build
source devel/setup.bash
cd ..
#####################
# DECLARE VARIABLES #
#####################
pwd
ls -lah
export SOURCE_DATE_EPOCH=$(git log -1 --pretty=%ct)
##############
# BUILD DOCS #
##############
# build our documentation with sphinx (see docs/conf.py)
# * https://www.sphinx-doc.org/en/master/usage/quickstart.html#running-the-build
make -C docs clean
make -C docs html
#######################
# Update GitHub Pages #
#######################
git config --global user.name "${GITHUB_ACTOR}"
git config --global user.email "${GITHUB_ACTOR}@users.noreply.github.com"
docroot=`mktemp -d`
rsync -av "docs/_build/html/" "${docroot}/"
pushd "${docroot}"
# don't bother maintaining history; just generate fresh
git init
git remote add deploy "https://token:${GITHUB_TOKEN}@github.com/${GITHUB_REPOSITORY}.git"
git checkout -b gh-pages
# add .nojekyll to the root so that github won't 404 on content added to dirs
# that start with an underscore (_), such as our "_content" dir..
touch .nojekyll
# Add README
cat > README.md <<EOF
# GitHub Pages Cache
Nothing to see here. The contents of this branch are essentially a cache that's not intended to be viewed on github.com.
If you're looking to update our documentation, check the relevant development branch's 'docs/' dir.
For more information on how this documentation is built using Sphinx, Read the Docs, and GitHub Actions/Pages, see:
* https://tech.michaelaltfield.net/2020/07/18/sphinx-rtd-github-pages-1
EOF
# copy the resulting html pages built from sphinx above to our new git repo
git add .
# commit all the new files
msg="Updating Docs for commit ${GITHUB_SHA} made on `date -d"@${SOURCE_DATE_EPOCH}" --iso-8601=seconds` from ${GITHUB_REF} by ${GITHUB_ACTOR}"
git commit -am "${msg}"
# overwrite the contents of the gh-pages branch on our github.com repo
git push deploy gh-pages --force
popd # return to main repo sandbox root
# exit cleanly
exit 0
================================================
FILE: docs/conf.py
================================================
# Configuration file for the Sphinx documentation builder.
#
# This file only contains a selection of the most common options. For a full
# list see the documentation:
# https://www.sphinx-doc.org/en/master/usage/configuration.html
# -- Path setup --------------------------------------------------------------
# If extensions (or modules to document with autodoc) are in another directory,
# add these directories to sys.path here. If the directory is relative to the
# documentation root, use os.path.abspath to make it absolute, like shown here.
#
import os
import sys
sys.path.insert(0, os.path.abspath('..'))
# -- Project information -----------------------------------------------------
project = 'frankapy'
copyright = '2021, Kevin Zhang, Mohit Sharma, Jacky Liang, Oliver Kroemer'
author = 'Kevin Zhang, Mohit Sharma, Jacky Liang, Oliver Kroemer'
# The full version, including alpha/beta/rc tags
release = '1.0.0'
# -- General configuration ---------------------------------------------------
# Add any Sphinx extension module names here, as strings. They can be
# extensions coming with Sphinx (named 'sphinx.ext.*') or your custom
# ones.
extensions = ['sphinx_rtd_theme',
'sphinx.ext.autosectionlabel',
'sphinx.ext.autodoc',
'sphinx.ext.napoleon',
'sphinx.ext.viewcode',
'sphinx.ext.githubpages',
]
autoclass_content = "class"
autodoc_member_order = "bysource"
autodoc_default_flags = ["members", "show-inheritance"]
#autodoc_mock_imports = ["numpy", "scipy", "matplotlib", "matplotlib.pyplot", "scipy.interpolate", "autolab_core", "quaternion", "itertools", "roslib", "rospy", "actionlib", "sensor_msgs", "franka_interface_msgs", "franka_gripper"]
napoleon_include_special_with_doc = True
napoleon_include_init_with_doc = True
# Add any paths that contain templates here, relative to this directory.
templates_path = ['_templates']
# The suffix(es) of source filenames.
# You can specify multiple suffix as a list of string:
#
# source_suffix = ['.rst', '.md']
source_suffix = '.rst'
# The master toctree document.
master_doc = 'index'
# List of patterns, relative to source directory, that match files and
# directories to ignore when looking for source files.
# This pattern also affects html_static_path and html_extra_path.
exclude_patterns = ['_build', 'Thumbs.db', '.DS_Store']
# -- Options for HTML output -------------------------------------------------
html_theme = 'sphinx_rtd_theme'
html_theme_path = []
# The theme to use for HTML and HTML Help pages. See the documentation for
# a list of builtin themes.
#
#html_theme = 'alabaster'
# Add any paths that contain custom static files (such as style sheets) here,
# relative to this directory. They are copied after the builtin static files,
# so a file named "default.css" will overwrite the builtin "default.css".
#html_static_path = ['_static']
================================================
FILE: docs/frankaarm.rst
================================================
Frankapy
========
FrankaArm
---------
.. autoclass:: frankapy.FrankaArm
:members:
================================================
FILE: docs/index.rst
================================================
.. frankapy documentation master file, created by
sphinx-quickstart on Sat Jun 5 14:43:41 2021.
You can adapt this file completely to your liking, but it should at least
contain the root `toctree` directive.
Welcome to FrankaPy's Documentation!
====================================
This library was designed to be used on a Ubuntu 18.04 / Ubuntu 20.04 PC with either ROS Melodic or ROS Noetic. We are currently not supporting ROS2 at the moment.
To join the Discord community, click the link `here <https://discord.gg/r6r7dttMwZ>`_.
If this library proves useful to your research, please cite the paper below::
@article{zhang2020modular,
title={A modular robotic arm control stack for research: Franka-interface and frankapy},
author={Zhang, Kevin and Sharma, Mohit and Liang, Jacky and Kroemer, Oliver},
journal={arXiv preprint arXiv:2011.02398},
year={2020}
}
Note that this library has been released with the Apache v2.0 license.
.. toctree::
:maxdepth: 2
install
network
running
frankaarm
support
Indices and tables
==================
* :ref:`genindex`
* :ref:`modindex`
* :ref:`search`
================================================
FILE: docs/install.rst
================================================
Installation
============
Requirements
------------
* A computer with Ubuntu 18.04 / 20.04
* ROS Melodic / Noetic
Steps
-----
1. Clone the Frankapy Repository and its Submodules::
git clone --recurse-submodules git@github.com:iamlab-cmu/frankapy.git
2. To allow asynchronous gripper commands, we use the ``franka_ros`` package, so install libfranka and franka_ros using the following command (Change melodic to noetic if you are on Ubuntu 20.04)::
sudo apt install ros-melodic-libfranka ros-melodic-franka-ros
3. Create and enter a virtual environment (:ref:`Virtual Environment`) and then run the following commands::
cd frankapy
pip install -e .
4. Compile the catkin_ws using the following script::
./bash_scripts/make_catkin.sh
5. Afterwards source the ``catkin_ws`` using the following command::
source catkin_ws/devel/setup.bash
6. It is a good idea to add the following line to the end of your ``~/.bashrc`` file::
source /path/to/frankapy/catkin_ws/devel/setup.bash --extend
(Optional) Additional Steps
---------------------------
Protobuf
~~~~~~~~
If you plan on modifying the library and the protobuf messages, you will need to compile the `Google Protocol Buffer <https://developers.google.com/protocol-buffers>`_ library from scratch using the following instructions.
1. First determine the number of cores on your computer using the command::
nproc
2. Execute the following commands::
sudo apt-get install autoconf automake libtool curl make g++ unzip
wget https://github.com/protocolbuffers/protobuf/releases/download/v21.8/protobuf-all-21.8.zip
unzip protobuf-all-21.8.zip
cd protobuf-21.8
./configure
3. Use the number that was previously printed out using the ``nproc`` command above and substitute it as ``N`` below::
make -jN
sudo make install
sudo ldconfig
4. Afterwards, you can make the protobuf messages using the following script::
./bash_scripts/make_proto.sh
Virtual Environment
~~~~~~~~~~~~~~~~~~~
Note that these instructions work on Ubuntu 18.04. They might be slightly different for Ubuntu 20.04.
1. Install Python3.6::
sudo apt install -y python3-distutils
2. Install Pip::
curl https://bootstrap.pypa.io/get-pip.py | sudo -H python3.6
3. Install Virtual Environment and Other Useful Python Packages::
sudo -H pip3.6 install numpy matplotlib virtualenv
4. Create a Virtual Environment for Frankapy::
virtualenv -p python3.6 franka
5. Enter into the Virtual Environment::
source franka/bin/activate
6. How to exit the Virtual Environment::
deactivate
================================================
FILE: docs/make.bat
================================================
@ECHO OFF
pushd %~dp0
REM Command file for Sphinx documentation
if "%SPHINXBUILD%" == "" (
set SPHINXBUILD=sphinx-build
)
set SOURCEDIR=.
set BUILDDIR=_build
if "%1" == "" goto help
%SPHINXBUILD% >NUL 2>NUL
if errorlevel 9009 (
echo.
echo.The 'sphinx-build' command was not found. Make sure you have Sphinx
echo.installed, then set the SPHINXBUILD environment variable to point
echo.to the full path of the 'sphinx-build' executable. Alternatively you
echo.may add the Sphinx directory to PATH.
echo.
echo.If you don't have Sphinx installed, grab it from
echo.http://sphinx-doc.org/
exit /b 1
)
%SPHINXBUILD% -M %1 %SOURCEDIR% %BUILDDIR% %SPHINXOPTS% %O%
goto end
:help
%SPHINXBUILD% -M help %SOURCEDIR% %BUILDDIR% %SPHINXOPTS% %O%
:end
popd
================================================
FILE: docs/network.rst
================================================
Network Configuration
=====================
If you are running franka-interface and frankapy on the same computer, you can skip this page.
Requirements
------------
* A Control PC computer running Ubuntu 18.04 with a Realtime Kernel. It should also have franka-interface installed with ROS Melodic.
Terminology
-----------
Control PC - Realtime Kernel PC connected by Ethernet to the Robot
FrankaPy PC - Computer running FrankaPy
Ethernet
--------
1. Plug an ethernet cable directly between the Control PC and the FrankaPy PC.
2. Go into the Ubuntu Network Connections Menu on the Control PC.
3. Select the Wired connection that corresponds to the Ethernet port that is connected to the FrankaPy PC. Then click the settings gear icon to edit.
4. Go to the IPv4 Settings Tab and switch from Automatic (DHCP) to Manual. Add a static ip address like 192.168.1.3 on the Control PC with netmask 24 and then click Apply.
.. image:: imgs/network_config.png
:width: 600
:alt: Network Config Photo
5. Then do the same on the FrankaPy PC but instead set the static ip address to be 192.168.1.2 with netmask 24.
Wifi
----
While FrankaPy will work over Wifi, it is not recommended due to additional latency in sending commands between computers.
1. If you are only communicating with the Control PC over Wifi, use the command ``ifconfig`` in order to get the Wifi IP address of both the Control PC and FrankaPy PC and note them down.
Editing the /etc/hosts file
---------------------------
1. Now that you have the IP addresses for both the Control PC and FrankaPy PC, you will need to edit the /etc/hosts files on both computers in order to allow communication between the 2 over ROS.
2. On the Control PC, run the command::
sudo gedit /etc/hosts
3. If you are using an Ethernet connection, then add the following above the line ``# The following lines are desirable for IPv6 capable hosts:`` ::
192.168.1.2 [frankapy-pc-name]
\
Otherwise substitute ``192.168.1.2`` with the IP address of the FrankaPy PC that you discovered using the command ``ifconfig``.
4. Afterwards, on the FrankaPy PC, again run the command ``sudo gedit /etc/hosts`` and add the line::
192.168.1.3 [control-pc-name]
\
Otherwise substitute ``192.168.1.3`` with the IP address of the Control PC that you discovered using the command ``ifconfig``.
5. Now you should be able to ssh from the FrankaPy PC to the Control PC using the command::
ssh [control-pc-username]@[control-pc-name]
Input password to control-pc.
Setting Up SSH Key to Control PC
--------------------------------
Generate a new SSH Key on the FrankaPy PC
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1. Generate a SSH key by executing the following commands or reading these `instructions <https://help.github.com/en/articles/generating-a-new-ssh-key-and-adding-it-to-the-ssh-agent>`_::
ssh-keygen -t rsa -b 4096 -C "your_email@example.com"
[Press enter]
[Press enter]
[Press enter]
eval "$(ssh-agent -s)"
ssh-add ~/.ssh/id_rsa
Upload the public SSH key to the Control PC
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1. In a new terminal on your FrankaPy PC, use your favorite text editor to open your ``id_rsa.pub`` file::
gedit ~/.ssh/id_rsa.pub
2. Copy the contents in your ``id_rsa.pub`` file.
3. Next, SSH to the Control PC::
ssh [control-pc-username]@[control-pc-name]
Input password to control-pc.
4. Use vim to open the authorized_keys file::
vim ~/.ssh/authorized_keys
5. Press the following buttons to paste your copied public key into the ``authorized_keys`` file on the Control PC::
i
ctrl-shift-v
<esc>
:
w
q
<enter>
6. Open a new terminal on the FrankaPy PC and try SSHing to the Control PC and it should no longer require a password.
7. (Optional) Upload your SSH key to Github by following instructions `here <https://help.github.com/en/articles/adding-a-new-ssh-key-to-your-github-account>`_.
================================================
FILE: docs/running.rst
================================================
Running the Robot
=================
Unlocking the Franka Robot
--------------------------
1. If you are running franka-interface and frankapy on the same computer, you can skip to step 2. If you have a FrankaPy PC and a Control PC, first ssh to the Control PC from the FrankaPy PC using SSH with option ``-X``::
ssh -X [control-pc-username]@[control-pc-name]
2. Open a web browser, either firefox or google chrome using the command line::
firefox
3. Go to ``172.16.0.2`` in the web browser.
4. Login to the Franka Desk GUI using the username and password that you used during the initial robot setup.
5. Unlock the robot by clicking the unlock button on the bottom right of the web interface.
6. If the robot has pink lights, press down on the e-stop and then release it and the robot should turn blue. If the robot is white, just release the e-stop and it should also turn blue.
Starting the FrankaPy Interface
-------------------------------
1. Make sure that the Franka Robot has been unlocked in the Franka Desk GUI and has blue lights.
2. Open up a new terminal and go to the frankapy directory.
3. If you are running franka-interface and frankapy on the same computer, run the following command::
bash ./bash_scripts/start_control_pc.sh -i localhost
4. Otherwise run the following command::
bash ./bash_scripts/start_control_pc.sh -u [control-pc-username] -i [control-pc-name]
5. Please see the ``bash_scripts/start_control_pc.sh`` bash script for additional arguments, including specifying a custom directory for where franka-interface is installed on the Control PC.
6. Open up a new terminal, enter into the same virtual environment that FrankaPy was installed in, go to the frankapy directory, then::
source catkin_ws/devel/setup.bash
7. Place your hand on top of the e-stop and reset the robot with the following command::
python scripts/reset_arm.py
8. See example scripts in the ``examples/`` and ``scripts/`` folders to learn how to use the FrankaPy python package.
9. Please note that if you are using a custom gripper or no gripper, please set the ``with_gripper=True`` flag in ``frankapy/franka_arm.py`` to ``False`` as well as set the ``with_gripper=1`` flag in ``bash_scripts/start_control_pc.sh`` to ``0``.
================================================
FILE: docs/support.rst
================================================
Support
=======
The easiest way to get help with the library is to join the FrankaPy and Franka-Interface Discord using this link `here <https://discord.gg/r6r7dttMwZ>`_.
================================================
FILE: examples/T_RSD415_franka.tf
================================================
realsense
franka_tool
-0.046490 0.030720 -0.083270
-0.000000 1.000000 0.000000
-1.000000 -0.000000 0.000000
0.000000 0.000000 1.000000
================================================
FILE: examples/T_RS_mount_delta.tf
================================================
franka_tool
franka_tool_base
0. 0. -0.005
1. 0. 0.
0. 1. 0.
0. 0. 1.
================================================
FILE: examples/april_tag_pick_place.py
================================================
import os
import logging
import argparse
from time import sleep
import numpy as np
from autolab_core import RigidTransform, YamlConfig
from visualization import Visualizer3D as vis3d
from perception_utils.apriltags import AprilTagDetector
from perception_utils.realsense import get_first_realsense_sensor
from frankapy import FrankaArm
def subsample(data, rate=0.1):
idx = np.random.choice(np.arange(len(data)), size=int(rate * len(data)))
return data[idx]
def make_det_one(R):
U, _, Vt = np.linalg.svd(R)
return U @ np.eye(len(R)) @ Vt
def get_closest_grasp_pose(T_tag_world, T_ee_world):
tag_axes = [
T_tag_world.rotation[:,0], -T_tag_world.rotation[:,0],
T_tag_world.rotation[:,1], -T_tag_world.rotation[:,1]
]
x_axis_ee = T_ee_world.rotation[:,0]
dots = [axis @ x_axis_ee for axis in tag_axes]
grasp_x_axis = tag_axes[np.argmax(dots)]
grasp_z_axis = np.array([0, 0, -1])
grasp_y_axis = np.cross(grasp_z_axis, grasp_x_axis)
grasp_R = make_det_one(np.c_[grasp_x_axis, grasp_y_axis, grasp_z_axis])
grasp_translation = T_tag_world.translation + np.array([0, 0, -cfg['cube_size'] / 2])
return RigidTransform(
rotation=grasp_R,
translation=grasp_translation,
from_frame=T_ee_world.from_frame, to_frame=T_ee_world.to_frame
)
if __name__ == "__main__":
logging.getLogger().setLevel(logging.INFO)
parser = argparse.ArgumentParser()
parser.add_argument('--cfg', '-c', type=str, default='cfg/april_tag_pick_place_cfg.yaml')
parser.add_argument('--no_grasp', '-ng', action='store_true')
args = parser.parse_args()
cfg = YamlConfig(args.cfg)
T_camera_ee = RigidTransform.load(cfg['T_camera_ee_path'])
T_camera_mount_delta = RigidTransform.load(cfg['T_camera_mount_path'])
logging.info('Starting robot')
fa = FrankaArm()
fa.set_tool_delta_pose(T_camera_mount_delta)
fa.reset_joints()
fa.open_gripper()
T_ready_world = fa.get_pose()
T_ready_world.translation[0] += 0.25
T_ready_world.translation[2] = 0.4
fa.goto_pose(T_ready_world)
logging.info('Init camera')
sensor = get_first_realsense_sensor(cfg['rs'])
sensor.start()
logging.info('Detecting April Tags')
april = AprilTagDetector(cfg['april_tag'])
intr = sensor.color_intrinsics
T_tag_camera = april.detect(sensor, intr, vis=cfg['vis_detect'])[0]
T_camera_world = T_ready_world * T_camera_ee
T_tag_world = T_camera_world * T_tag_camera
logging.info('Tag has translation {}'.format(T_tag_world.translation))
logging.info('Finding closest orthogonal grasp')
T_grasp_world = get_closest_grasp_pose(T_tag_world, T_ready_world)
T_lift = RigidTransform(translation=[0, 0, 0.2], from_frame=T_ready_world.to_frame, to_frame=T_ready_world.to_frame)
T_lift_world = T_lift * T_grasp_world
logging.info('Visualizing poses')
_, depth_im, _ = sensor.frames()
points_world = T_camera_world * intr.deproject(depth_im)
if cfg['vis_detect']:
vis3d.figure()
vis3d.pose(RigidTransform())
vis3d.points(subsample(points_world.data.T), color=(0,1,0), scale=0.002)
vis3d.pose(T_ready_world)
vis3d.pose(T_camera_world)
vis3d.pose(T_tag_world)
vis3d.pose(T_grasp_world)
vis3d.pose(T_lift_world)
vis3d.show()
if not args.no_grasp:
logging.info('Commanding robot')
fa.goto_pose(T_lift_world, use_impedance=False)
fa.goto_pose(T_grasp_world, use_impedance=False)
fa.close_gripper()
fa.goto_pose(T_lift_world, use_impedance=False)
sleep(3)
fa.goto_pose(T_grasp_world, use_impedance=False)
fa.open_gripper()
fa.goto_pose(T_lift_world, use_impedance=False)
fa.goto_pose(T_ready_world, use_impedance=False)
import IPython; IPython.embed(); exit(0)
================================================
FILE: examples/example_movements.py
================================================
from frankapy import FrankaArm
import numpy as np
from autolab_core import RigidTransform
if __name__ == '__main__':
print('Starting robot')
fa = FrankaArm()
xtranslation_3cm = RigidTransform(rotation=np.array([
[1, 0, 0],
[0, 1, 0],
[0, 0, 1]
]), translation=np.array([0.03, 0, 0]),
from_frame='franka_tool', to_frame='world')
random_position = RigidTransform(rotation=np.array([
[0.9323473, -0.35858258, 0.04612846],
[-0.35996283, -0.93259467, 0.02597504],
[0.03370496, -0.04082229, -0.99859775]
]), translation=np.array([0.39247965, -0.21613652, 0.3882055]),
from_frame='franka_tool', to_frame='world')
print(fa.get_pose().translation)
print(fa.get_joints())
desired_joints_1 = [-0.52733715, 0.25603565, 0.47721503, -1.26705864, 0.00600359, 1.60788199, 0.63019184]
desired_joints_2 = [-0.16017485, 1.12476619, 0.26004398, -0.67246923, 0.04899213, 2.08439578, 0.81627789]
fa.reset_joints()
print('Opening Grippers')
fa.open_gripper()
#fa.reset_pose()
# fa.goto_pose(random_position, use_impedance=False, cartesian_impedances=[3000, 3000, 100, 300, 300, 300])
fa.goto_joints(desired_joints_1, joint_impedances=[100, 100, 100, 50, 50, 100, 100])
fa.goto_joints(desired_joints_2, joint_impedances=[100, 100, 100, 50, 50, 100, 100])
#fa.apply_effector_forces_torques(10.0, 0, 0, 0)
================================================
FILE: examples/execute_joint_torques.py
================================================
import argparse
from frankapy import FrankaArm
if __name__ == '__main__':
parser = argparse.ArgumentParser()
parser.add_argument('--time', '-t', type=float, default=10)
args = parser.parse_args()
print('Starting robot')
fa = FrankaArm()
fa.reset_joints()
print('Applying 0 joint torque control for {}s'.format(args.time))
fa.execute_joint_torques([0,0,0,0,0,3,0], selection=[0,0,0,0,0,1,0], remove_gravity=[0,0,0,0,0,1,0], duration=args.time)
================================================
FILE: examples/go_to_joints_with_joint_impedances.py
================================================
import argparse
import sys
from frankapy import FrankaArm
from frankapy import FrankaConstants as FC
def wait_for_enter():
if sys.version_info[0] < 3:
raw_input('Press Enter to continue:')
else:
input('Press Enter to continue:')
if __name__ == '__main__':
parser = argparse.ArgumentParser()
parser.add_argument('--time', '-t', type=float, default=10)
parser.add_argument('--open_gripper', '-o', action='store_true')
args = parser.parse_args()
print('Starting robot')
fa = FrankaArm()
if args.open_gripper:
fa.open_gripper()
print('Be very careful!! Make sure the robot can safely move to HOME JOINTS Position.')
wait_for_enter()
fa.reset_joints()
print('Using default joint impedances to move back and forth.')
wait_for_enter()
fa.goto_joints(FC.READY_JOINTS, joint_impedances=FC.DEFAULT_JOINT_IMPEDANCES)
fa.goto_joints(FC.HOME_JOINTS)
print('Now using different joint impedances to move back and forth.')
wait_for_enter()
fa.goto_joints(FC.READY_JOINTS, joint_impedances=[1500, 1500, 1500, 1250, 1250, 1000, 1000])
fa.goto_joints(FC.HOME_JOINTS)
print('Remember to reset the joint_impedances to defaults.')
fa.goto_joints(FC.HOME_JOINTS, joint_impedances=FC.DEFAULT_JOINT_IMPEDANCES)
================================================
FILE: examples/move_robot.py
================================================
import numpy as np
from autolab_core import RigidTransform
from frankapy import FrankaArm
if __name__ == "__main__":
fa = FrankaArm()
# reset franka to its home joints
fa.reset_joints()
# read functions
T_ee_world = fa.get_pose()
print('Translation: {} | Rotation: {}'.format(T_ee_world.translation, T_ee_world.quaternion))
joints = fa.get_joints()
print('Joints: {}'.format(joints))
gripper_width = fa.get_gripper_width()
print('Gripper width: {}'.format(gripper_width))
# gripper controls
print('Closing gripper')
fa.close_gripper()
print('Opening gripper to a specified position')
fa.goto_gripper(0.02)
print('Opening gripper all the way')
fa.open_gripper()
# joint controls
print('Rotating last joint')
joints = fa.get_joints()
joints[6] += np.deg2rad(45)
fa.goto_joints(joints)
joints[6] -= np.deg2rad(45)
fa.goto_joints(joints)
# end-effector pose control
print('Translation')
T_ee_world = fa.get_pose()
T_ee_world.translation += [0.1, 0, 0.1]
fa.goto_pose(T_ee_world)
T_ee_world.translation -= [0.1, 0, 0.1]
fa.goto_pose(T_ee_world)
print('Rotation in end-effector frame')
T_ee_rot = RigidTransform(
rotation=RigidTransform.x_axis_rotation(np.deg2rad(45)),
from_frame='franka_tool', to_frame='franka_tool'
)
T_ee_world_target = T_ee_world * T_ee_rot
fa.goto_pose(T_ee_world_target)
fa.goto_pose(T_ee_world)
# reset franka back to home
fa.reset_joints()
================================================
FILE: examples/record_trajectory.py
================================================
import argparse
import time
from frankapy import FrankaArm
import pickle as pkl
if __name__ == '__main__':
parser = argparse.ArgumentParser()
parser.add_argument('--time', '-t', type=float, default=10)
parser.add_argument('--open_gripper', '-o', action='store_true')
parser.add_argument('--file', '-f', default='franka_traj.pkl')
args = parser.parse_args()
print('Starting robot')
fa = FrankaArm()
if args.open_gripper:
fa.open_gripper()
print('Applying 0 force torque control for {}s'.format(args.time))
end_effector_position = []
fa.run_guide_mode(args.time, block=False)
for i in range(1000):
end_effector_position.append(fa.get_pose())
time.sleep(0.01)
pkl.dump(end_effector_position, open(args.file, 'wb'))
================================================
FILE: examples/run_dynamic_cartesian_velocities.py
================================================
import numpy as np
import time
from frankapy import FrankaConstants as FC
from frankapy import FrankaArm, SensorDataMessageType
from frankapy.proto_utils import sensor_proto2ros_msg, make_sensor_group_msg
from frankapy.proto import CartesianVelocitySensorMessage, ShouldTerminateSensorMessage
from franka_interface_msgs.msg import SensorDataGroup
from frankapy.utils import min_jerk
import rospy
if __name__ == "__main__":
fa = FrankaArm()
fa.reset_joints()
rospy.loginfo('Generating Trajectory')
p = fa.get_pose()
p.translation[2] -= 0.2
fa.goto_pose(p, duration=5, block=False)
cartesian_accelerations = [1, 1, 1, 0.1, 0.1, 0.1]
T = 5
dt = 0.01
ts = np.arange(0, T, dt)
cartesian_velocities = []
for i in range(len(ts)):
cartesian_velocities.append(fa.get_robot_state()['cartesian_velocities'])
time.sleep(dt)
fa.wait_for_skill()
print(cartesian_velocities)
pub = rospy.Publisher(FC.DEFAULT_SENSOR_PUBLISHER_TOPIC, SensorDataGroup, queue_size=1000)
rate = rospy.Rate(1 / dt)
fa.reset_joints()
rospy.loginfo('Initializing Sensor Publisher')
rospy.loginfo('Publishing cartesian velocity trajectory...')
# To ensure skill doesn't end before completing trajectory, make the buffer time much longer than needed
fa.execute_cartesian_velocities(cartesian_velocities[0], cartesian_accelerations, duration=T, dynamic=True, buffer_time=10)
init_time = rospy.Time.now().to_time()
for i in range(len(ts)):
traj_gen_proto_msg = CartesianVelocitySensorMessage(
id=i, timestamp=rospy.Time.now().to_time() - init_time,
cartesian_velocities=cartesian_velocities[i]
)
ros_msg = make_sensor_group_msg(
trajectory_generator_sensor_msg=sensor_proto2ros_msg(
traj_gen_proto_msg, SensorDataMessageType.CARTESIAN_VELOCITY)
)
rospy.loginfo('Publishing: ID {}'.format(traj_gen_proto_msg.id))
pub.publish(ros_msg)
time.sleep(dt)
# Stop the skill
# Alternatively can call fa.stop_skill()
term_proto_msg = ShouldTerminateSensorMessage(timestamp=rospy.Time.now().to_time() - init_time, should_terminate=True)
ros_msg = make_sensor_group_msg(
termination_handler_sensor_msg=sensor_proto2ros_msg(
term_proto_msg, SensorDataMessageType.SHOULD_TERMINATE)
)
pub.publish(ros_msg)
rospy.loginfo('Done')
================================================
FILE: examples/run_dynamic_joint_velocities.py
================================================
import numpy as np
import time
from frankapy import FrankaConstants as FC
from frankapy import FrankaArm, SensorDataMessageType
from frankapy.proto_utils import sensor_proto2ros_msg, make_sensor_group_msg
from frankapy.proto import JointVelocitySensorMessage, ShouldTerminateSensorMessage
from franka_interface_msgs.msg import SensorDataGroup
from frankapy.utils import min_jerk
import rospy
if __name__ == "__main__":
fa = FrankaArm()
fa.reset_joints()
rospy.loginfo('Generating Trajectory')
p = fa.get_pose()
p.translation[2] -= 0.2
fa.goto_pose(p, duration=5, block=False)
joint_accelerations = [1, 1, 1, 1, 1, 1, 1]
T = 5
dt = 0.01
ts = np.arange(0, T, dt)
joint_velocities = []
for i in range(len(ts)):
joint_velocities.append(fa.get_robot_state()['joint_velocities'])
time.sleep(dt)
fa.wait_for_skill()
pub = rospy.Publisher(FC.DEFAULT_SENSOR_PUBLISHER_TOPIC, SensorDataGroup, queue_size=1000)
rate = rospy.Rate(1 / dt)
fa.reset_joints()
rospy.loginfo('Initializing Sensor Publisher')
rospy.loginfo('Publishing joints trajectory...')
# To ensure skill doesn't end before completing trajectory, make the buffer time much longer than needed
fa.execute_joint_velocities(joint_velocities[0], joint_accelerations, duration=T, dynamic=True, buffer_time=10)
init_time = rospy.Time.now().to_time()
for i in range(len(ts)):
traj_gen_proto_msg = JointVelocitySensorMessage(
id=i, timestamp=rospy.Time.now().to_time() - init_time,
joint_velocities=joint_velocities[i]
)
ros_msg = make_sensor_group_msg(
trajectory_generator_sensor_msg=sensor_proto2ros_msg(
traj_gen_proto_msg, SensorDataMessageType.JOINT_VELOCITY)
)
rospy.loginfo('Publishing: ID {}'.format(traj_gen_proto_msg.id))
pub.publish(ros_msg)
time.sleep(dt)
# Stop the skill
# Alternatively can call fa.stop_skill()
term_proto_msg = ShouldTerminateSensorMessage(timestamp=rospy.Time.now().to_time() - init_time, should_terminate=True)
ros_msg = make_sensor_group_msg(
termination_handler_sensor_msg=sensor_proto2ros_msg(
term_proto_msg, SensorDataMessageType.SHOULD_TERMINATE)
)
pub.publish(ros_msg)
rospy.loginfo('Done')
================================================
FILE: examples/run_dynamic_joints.py
================================================
import numpy as np
from frankapy import FrankaArm, SensorDataMessageType
from frankapy import FrankaConstants as FC
from frankapy.proto_utils import sensor_proto2ros_msg, make_sensor_group_msg
from frankapy.proto import JointPositionSensorMessage, ShouldTerminateSensorMessage
from franka_interface_msgs.msg import SensorDataGroup
from frankapy.utils import min_jerk
import rospy
if __name__ == "__main__":
fa = FrankaArm()
fa.reset_joints()
rospy.loginfo('Generating Trajectory')
joints_0 = fa.get_joints()
p = fa.get_pose()
p.translation[2] -= 0.2
fa.goto_pose(p)
joints_1 = fa.get_joints()
T = 5
dt = 0.02
ts = np.arange(0, T, dt)
joints_traj = [min_jerk(joints_1, joints_0, t, T) for t in ts]
rospy.loginfo('Initializing Sensor Publisher')
pub = rospy.Publisher(FC.DEFAULT_SENSOR_PUBLISHER_TOPIC, SensorDataGroup, queue_size=1000)
rate = rospy.Rate(1 / dt)
rospy.loginfo('Publishing joints trajectory...')
# To ensure skill doesn't end before completing trajectory, make the buffer time much longer than needed
fa.goto_joints(joints_traj[1], duration=T, dynamic=True, buffer_time=10)
init_time = rospy.Time.now().to_time()
for i in range(2, len(ts)):
traj_gen_proto_msg = JointPositionSensorMessage(
id=i, timestamp=rospy.Time.now().to_time() - init_time,
joints=joints_traj[i]
)
ros_msg = make_sensor_group_msg(
trajectory_generator_sensor_msg=sensor_proto2ros_msg(
traj_gen_proto_msg, SensorDataMessageType.JOINT_POSITION)
)
rospy.loginfo('Publishing: ID {}'.format(traj_gen_proto_msg.id))
pub.publish(ros_msg)
rate.sleep()
# Stop the skill
# Alternatively can call fa.stop_skill()
term_proto_msg = ShouldTerminateSensorMessage(timestamp=rospy.Time.now().to_time() - init_time, should_terminate=True)
ros_msg = make_sensor_group_msg(
termination_handler_sensor_msg=sensor_proto2ros_msg(
term_proto_msg, SensorDataMessageType.SHOULD_TERMINATE)
)
pub.publish(ros_msg)
rospy.loginfo('Done')
================================================
FILE: examples/run_dynamic_pose.py
================================================
import numpy as np
from autolab_core import RigidTransform
from frankapy import FrankaArm, SensorDataMessageType
from frankapy import FrankaConstants as FC
from frankapy.proto_utils import sensor_proto2ros_msg, make_sensor_group_msg
from frankapy.proto import PosePositionSensorMessage, ShouldTerminateSensorMessage, CartesianImpedanceSensorMessage
from franka_interface_msgs.msg import SensorDataGroup
from frankapy.utils import min_jerk, min_jerk_weight
import rospy
if __name__ == "__main__":
fa = FrankaArm()
fa.reset_joints()
rospy.loginfo('Generating Trajectory')
p0 = fa.get_pose()
p1 = p0.copy()
T_delta = RigidTransform(
translation=np.array([0, 0, 0.2]),
rotation=RigidTransform.z_axis_rotation(np.deg2rad(30)),
from_frame=p1.from_frame, to_frame=p1.from_frame)
p1 = p1 * T_delta
fa.goto_pose(p1)
T = 5
dt = 0.02
ts = np.arange(0, T, dt)
has_closed = False
weights = [min_jerk_weight(t, T) for t in ts]
pose_traj = [p1.interpolate_with(p0, w) for w in weights]
z_stiffness_traj = [min_jerk(100, 800, t, T) for t in ts]
rospy.loginfo('Initializing Sensor Publisher')
pub = rospy.Publisher(FC.DEFAULT_SENSOR_PUBLISHER_TOPIC, SensorDataGroup, queue_size=1000)
rate = rospy.Rate(1 / dt)
rospy.loginfo('Publishing pose trajectory...')
# To ensure skill doesn't end before completing trajectory, make the buffer time much longer than needed
fa.goto_pose(pose_traj[1], duration=T, dynamic=True, buffer_time=10,
cartesian_impedances=FC.DEFAULT_TRANSLATIONAL_STIFFNESSES[:2] + [z_stiffness_traj[1]] + FC.DEFAULT_ROTATIONAL_STIFFNESSES
)
init_time = rospy.Time.now().to_time()
for i in range(2, len(ts)):
timestamp = rospy.Time.now().to_time() - init_time
traj_gen_proto_msg = PosePositionSensorMessage(
id=i, timestamp=timestamp,
position=pose_traj[i].translation, quaternion=pose_traj[i].quaternion
)
fb_ctrlr_proto = CartesianImpedanceSensorMessage(
id=i, timestamp=timestamp,
translational_stiffnesses=FC.DEFAULT_TRANSLATIONAL_STIFFNESSES[:2] + [z_stiffness_traj[i]],
rotational_stiffnesses=FC.DEFAULT_ROTATIONAL_STIFFNESSES
)
ros_msg = make_sensor_group_msg(
trajectory_generator_sensor_msg=sensor_proto2ros_msg(
traj_gen_proto_msg, SensorDataMessageType.POSE_POSITION),
feedback_controller_sensor_msg=sensor_proto2ros_msg(
fb_ctrlr_proto, SensorDataMessageType.CARTESIAN_IMPEDANCE)
)
rospy.loginfo('Publishing: ID {}'.format(traj_gen_proto_msg.id))
pub.publish(ros_msg)
rate.sleep()
# Stop the skill
# Alternatively can call fa.stop_skill()
term_proto_msg = ShouldTerminateSensorMessage(timestamp=rospy.Time.now().to_time() - init_time, should_terminate=True)
ros_msg = make_sensor_group_msg(
termination_handler_sensor_msg=sensor_proto2ros_msg(
term_proto_msg, SensorDataMessageType.SHOULD_TERMINATE)
)
pub.publish(ros_msg)
rospy.loginfo('Done')
================================================
FILE: examples/run_hfpc.py
================================================
import numpy as np
from frankapy import FrankaArm, SensorDataMessageType
from frankapy import FrankaConstants as FC
from frankapy.proto_utils import sensor_proto2ros_msg, make_sensor_group_msg
from frankapy.proto import ForcePositionSensorMessage, ForcePositionControllerSensorMessage
from franka_interface_msgs.msg import SensorDataGroup
from frankapy.utils import transform_to_list, min_jerk
from tqdm import trange
import rospy
if __name__ == "__main__":
fa = FrankaArm()
fa.reset_joints()
fa.close_gripper()
while True:
input('Presse [Enter] to enter guide mode and move robot to be on top of a flat surface.')
fa.run_guide_mode()
while True:
inp = input('[r]etry or [c]ontinue? ')
if inp not in ('r', 'c'):
print('Please give valid input!')
else:
break
if inp == 'c':
break
rospy.loginfo('Generating Trajectory')
# EE will follow a 2D circle while pressing down with a target force
dt = 0.01
T = 10
ts = np.arange(0, T, dt)
N = len(ts)
dthetas = np.linspace(-np.pi / 2, 3 * np.pi / 2, N)
r = 0.07
circ = r * np.c_[np.sin(dthetas), np.cos(dthetas)]
start_pose = fa.get_pose()
start_pose.rotation = FC.HOME_POSE.rotation
target_poses = []
for i, t in enumerate(ts):
pose = start_pose.copy()
pose.translation[0] += r + circ[i, 0]
pose.translation[1] += circ[i, 1]
target_poses.append(pose)
target_force = [0, 0, -10, 0, 0, 0]
S = [1, 1, 1, 1, 1, 1]
position_kps_cart = FC.DEFAULT_TRANSLATIONAL_STIFFNESSES + FC.DEFAULT_ROTATIONAL_STIFFNESSES
force_kps_cart = [0.1] * 6
position_kps_joint = FC.DEFAULT_K_GAINS
force_kps_joint = [0.1] * 7
rospy.loginfo('Initializing Sensor Publisher')
pub = rospy.Publisher(FC.DEFAULT_SENSOR_PUBLISHER_TOPIC, SensorDataGroup, queue_size=1)
rate = rospy.Rate(1 / dt)
n_times = 1
rospy.loginfo('Publishing HFPC trajectory w/ cartesian gains...')
fa.run_dynamic_force_position(duration=T * n_times, buffer_time=3, S=S,
use_cartesian_gains=True,
position_kps_cart=position_kps_cart,
force_kps_cart=force_kps_cart)
init_time = rospy.Time.now().to_time()
for i in trange(N * n_times):
t = i % N
timestamp = rospy.Time.now().to_time() - init_time
traj_gen_proto_msg = ForcePositionSensorMessage(
id=i, timestamp=timestamp, seg_run_time=dt,
pose=transform_to_list(target_poses[t]),
force=target_force
)
fb_ctrlr_proto = ForcePositionControllerSensorMessage(
id=i, timestamp=timestamp,
position_kps_cart=position_kps_cart,
force_kps_cart=force_kps_cart,
selection=S
)
ros_msg = make_sensor_group_msg(
trajectory_generator_sensor_msg=sensor_proto2ros_msg(
traj_gen_proto_msg, SensorDataMessageType.FORCE_POSITION),
feedback_controller_sensor_msg=sensor_proto2ros_msg(
fb_ctrlr_proto, SensorDataMessageType.FORCE_POSITION_GAINS)
)
pub.publish(ros_msg)
rate.sleep()
fa.stop_skill()
rospy.loginfo('Publishing HFPC trajectory w/ joint gains...')
fa.run_dynamic_force_position(duration=T * n_times, buffer_time=3, S=S,
use_cartesian_gains=False,
position_kps_joint=position_kps_joint,
force_kps_joint=force_kps_joint)
init_time = rospy.Time.now().to_time()
for i in trange(N * n_times):
t = i % N
timestamp = rospy.Time.now().to_time() - init_time
traj_gen_proto_msg = ForcePositionSensorMessage(
id=i, timestamp=timestamp, seg_run_time=dt,
pose=transform_to_list(target_poses[t]),
force=target_force
)
fb_ctrlr_proto = ForcePositionControllerSensorMessage(
id=i, timestamp=timestamp,
position_kps_joint=position_kps_joint,
force_kps_joint=force_kps_joint,
selection=S
)
ros_msg = make_sensor_group_msg(
trajectory_generator_sensor_msg=sensor_proto2ros_msg(
traj_gen_proto_msg, SensorDataMessageType.FORCE_POSITION),
feedback_controller_sensor_msg=sensor_proto2ros_msg(
fb_ctrlr_proto, SensorDataMessageType.FORCE_POSITION_GAINS)
)
pub.publish(ros_msg)
rate.sleep()
fa.stop_skill()
fa.reset_joints()
fa.open_gripper()
rospy.loginfo('Done')
================================================
FILE: examples/run_pose_dmp.py
================================================
import numpy as np
import math
import rospy
import argparse
import pickle
from autolab_core import RigidTransform, Point
from frankapy import FrankaArm
if __name__ == '__main__':
parser = argparse.ArgumentParser()
parser.add_argument('--pose_dmp_weights_file_path', '-f', type=str)
args = parser.parse_args()
print('Starting robot')
fa = FrankaArm();
with open(args.pose_dmp_weights_file_path, 'rb') as pkl_f:
pose_dmp_info = pickle.load(pkl_f)
fa.execute_pose_dmp(pose_dmp_info, duration=8)
================================================
FILE: examples/run_quaternion_pose_dmp.py
================================================
import numpy as np
import math
import rospy
import argparse
import pickle
from autolab_core import RigidTransform, Point
from frankapy import FrankaArm
def execute_quaternion_pose_dmp(fa, position_dmp_weights_path, quat_dmp_weights_path,
goal_quat=(0.03, 1.0, -0.03, 0.01),
duration=10.0):
position_dmp_file = open(position_dmp_weights_path, 'rb')
position_dmp_info = pickle.load(position_dmp_file)
quat_dmp_file = open(quat_dmp_weights_path, 'rb')
quat_dmp_info = pickle.load(quat_dmp_file)
# Should be less than duration so that the canonical system is set to 0 appropriately
quat_goal_time = duration - 3.0
fa.execute_quaternion_pose_dmp(position_dmp_info, quat_dmp_info, duration, goal_quat, quat_goal_time)
if __name__ == '__main__':
parser = argparse.ArgumentParser()
parser.add_argument('--position_dmp_weights_path', '-p', type=str)
parser.add_argument('--quat_dmp_weights_path', '-q', type=str)
args = parser.parse_args()
print('Starting robot')
fa = FrankaArm();
execute_quaternion_pose_dmp(fa,
args.position_dmp_weights_path,
args.quat_dmp_weights_path,
goal_quat=(0.03, 1.0, -0.03, 0.01),
duration=20.0):
================================================
FILE: examples/run_recorded_trajectory.py
================================================
import pickle as pkl
import numpy as np
from autolab_core import RigidTransform
from frankapy import FrankaArm, SensorDataMessageType
from frankapy import FrankaConstants as FC
from frankapy.proto_utils import sensor_proto2ros_msg, make_sensor_group_msg
from frankapy.proto import PosePositionSensorMessage, ShouldTerminateSensorMessage, CartesianImpedanceSensorMessage
from franka_interface_msgs.msg import SensorDataGroup
from frankapy.utils import min_jerk, min_jerk_weight
import rospy
if __name__ == "__main__":
fa = FrankaArm()
fa.reset_joints()
rospy.loginfo('Generating Trajectory')
pose_traj = pkl.load(open('franka_traj.pkl','rb'))
T = 10
dt = 0.01
ts = np.arange(0, T, dt)
rospy.loginfo('Initializing Sensor Publisher')
pub = rospy.Publisher(FC.DEFAULT_SENSOR_PUBLISHER_TOPIC, SensorDataGroup, queue_size=10)
rate = rospy.Rate(1 / dt)
rospy.loginfo('Publishing pose trajectory...')
# To ensure skill doesn't end before completing trajectory, make the buffer time much longer than needed
fa.goto_pose(pose_traj[1], duration=T, dynamic=True, buffer_time=10,
cartesian_impedances=[600.0, 600.0, 600.0, 50.0, 50.0, 50.0]
)
init_time = rospy.Time.now().to_time()
for i in range(2, len(ts)):
timestamp = rospy.Time.now().to_time() - init_time
traj_gen_proto_msg = PosePositionSensorMessage(
id=i, timestamp=timestamp,
position=pose_traj[i].translation, quaternion=pose_traj[i].quaternion
)
ros_msg = make_sensor_group_msg(
trajectory_generator_sensor_msg=sensor_proto2ros_msg(
traj_gen_proto_msg, SensorDataMessageType.POSE_POSITION),
)
rospy.loginfo('Publishing: ID {}'.format(traj_gen_proto_msg.id))
pub.publish(ros_msg)
rate.sleep()
# Stop the skill
# Alternatively can call fa.stop_skill()
term_proto_msg = ShouldTerminateSensorMessage(timestamp=rospy.Time.now().to_time() - init_time, should_terminate=True)
ros_msg = make_sensor_group_msg(
termination_handler_sensor_msg=sensor_proto2ros_msg(
term_proto_msg, SensorDataMessageType.SHOULD_TERMINATE)
)
pub.publish(ros_msg)
rospy.loginfo('Done')
================================================
FILE: frankapy/__init__.py
================================================
from .franka_arm import FrankaArm
from .franka_constants import FrankaConstants
from .franka_arm_state_client import FrankaArmStateClient
from .exceptions import FrankaArmCommException
from .franka_interface_common_definitions import SkillType, MetaSkillType, TrajectoryGeneratorType, FeedbackControllerType, TerminationHandlerType, SkillStatus, SensorDataMessageType
================================================
FILE: frankapy/exceptions.py
================================================
class FrankaArmCommException(Exception):
''' Communication failure. Usually occurs due to timeouts.
'''
def __init__(self, message, *args, **kwargs):
Exception.__init__(self, *args, **kwargs)
self.message = message
def __str__(self):
return "Communication w/ FrankaInterface ran into a problem: {}. FrankaInterface is probably not ready.".format(self.message)
class FrankaArmFrankaInterfaceNotReadyException(Exception):
''' Exception for when franka_interface is not ready
'''
def __init__(self, *args, **kwargs):
Exception.__init__(self, *args, **kwargs)
def __str__(self):
return 'FrankaInterface was not ready!'
class FrankaArmException(Exception):
''' Failure of control, typically due to a kinematically unreachable pose.
'''
def __init__(self, message, *args, **kwargs):
Exception.__init__(self, *args, **kwargs)
self.message = message
def __str__(self):
return "FrankaInterface ran into a problem: {}".format(self.message)
================================================
FILE: frankapy/franka_arm.py
================================================
"""
franka_arm.py
====================================
The core module of frankapy
"""
import sys, signal, logging
from time import time, sleep
import numpy as np
from autolab_core import RigidTransform
import quaternion
from itertools import product
import roslib
roslib.load_manifest('franka_interface_msgs')
import rospy
from actionlib import SimpleActionClient
from sensor_msgs.msg import JointState
from franka_interface_msgs.msg import ExecuteSkillAction, SensorDataGroup
from franka_interface_msgs.srv import GetCurrentFrankaInterfaceStatusCmd
from franka_gripper.msg import *
from .skill_list import *
from .exceptions import *
from .franka_arm_state_client import FrankaArmStateClient
from .franka_constants import FrankaConstants as FC
from .franka_interface_common_definitions import *
from .ros_utils import BoxesPublisher
class FrankaArm:
def __init__(
self,
rosnode_name='franka_arm_client',
ros_log_level=rospy.INFO,
robot_num=1,
with_gripper=True,
old_gripper=False,
offline=False,
init_node=True):
"""
Initialize a FrankaArm.
Parameters
----------
rosnode_name : :obj:`str`
A name for the FrankaArm ROS Node.
ros_log_level : :obj:`rospy.log_level` of int
Passed to rospy.init_node.
robot_num : :obj:`int`
Number assigned to the current robot.
with_gripper : :obj:`bool`
Flag for whether the Franka gripper is attached.
old_gripper : :obj:`bool`
Flag for whether to run the franka_ros gripper or the old_gripper
implementation.
offline : :obj:`bool`
Flag for whether to run the robot in the real world.
"""
self._execute_skill_action_server_name = \
'/execute_skill_action_server_node_{}/execute_skill'.format(robot_num)
self._robot_state_server_name = \
'/get_current_robot_state_server_node_{}/get_current_robot_state_server'.format(robot_num)
self._franka_interface_status_server_name = \
'/get_current_franka_interface_status_server_node_{}/get_current_franka_interface_status_server'.format(robot_num)
self._gripper_homing_action_server_name = \
'/franka_gripper_{}/homing'.format(robot_num)
self._gripper_move_action_server_name = \
'/franka_gripper_{}/move'.format(robot_num)
self._gripper_stop_action_server_name = \
'/franka_gripper_{}/stop'.format(robot_num)
self._gripper_grasp_action_server_name = \
'/franka_gripper_{}/grasp'.format(robot_num)
self._gripper_joint_states_name = \
'/franka_gripper_{}/joint_states'.format(robot_num)
if robot_num == 1:
self._sensor_publisher_name = \
'/franka_ros_interface/sensor'
else:
self._sensor_publisher_name = \
'/franka_ros_interface_{}/sensor'.format(robot_num)
self._connected = False
self._in_skill = False
self._offline = offline
self._with_gripper = with_gripper
self._old_gripper = old_gripper
self._last_gripper_command = None
# init ROS
if init_node:
rospy.init_node(rosnode_name,
disable_signals=True,
log_level=ros_log_level)
self._collision_boxes_pub = BoxesPublisher('franka_collision_boxes_{}'.format(robot_num))
self._joint_state_pub = rospy.Publisher('franka_virtual_joints_{}'.format(robot_num), JointState, queue_size=10)
self._sensor_pub = rospy.Publisher(self._sensor_publisher_name, SensorDataGroup, queue_size=100)
self._state_client = FrankaArmStateClient(
new_ros_node=False,
robot_state_server_name=self._robot_state_server_name,
offline=self._offline)
if not self._offline:
# set signal handler to handle ctrl+c and kill sigs
signal.signal(signal.SIGINT, self._sigint_handler_gen())
rospy.wait_for_service(self._franka_interface_status_server_name)
self._get_current_franka_interface_status = rospy.ServiceProxy(
self._franka_interface_status_server_name, GetCurrentFrankaInterfaceStatusCmd)
self._client = SimpleActionClient(
self._execute_skill_action_server_name, ExecuteSkillAction)
self._client.wait_for_server()
self.wait_for_franka_interface()
if self._with_gripper and not self._old_gripper:
self._gripper_homing_client = SimpleActionClient(
self._gripper_homing_action_server_name, HomingAction)
self._gripper_homing_client.wait_for_server()
self._gripper_move_client = SimpleActionClient(
self._gripper_move_action_server_name, MoveAction)
self._gripper_move_client.wait_for_server()
self._gripper_grasp_client = SimpleActionClient(
self._gripper_grasp_action_server_name, GraspAction)
self._gripper_grasp_client.wait_for_server()
self._gripper_stop_client = SimpleActionClient(
self._gripper_stop_action_server_name, StopAction)
self._gripper_stop_client.wait_for_server()
# done init ROS
self._connected = True
# set default identity tool delta pose
self._tool_delta_pose = RigidTransform(from_frame='franka_tool',
to_frame='franka_tool_base')
# Precompute things and preallocate np memory for collision checking
self._collision_boxes_data = np.zeros((len(FC.COLLISION_BOX_SHAPES), 10))
self._collision_boxes_data[:, -3:] = FC.COLLISION_BOX_SHAPES
self._collision_box_hdiags = []
self._collision_box_vertices_offset = []
self._vertex_offset_signs = np.array(list(product([1, -1],[1,-1], [1,-1])))
for sizes in FC.COLLISION_BOX_SHAPES:
hsizes = sizes/2
self._collision_box_vertices_offset.append(self._vertex_offset_signs * hsizes)
self._collision_box_hdiags.append(np.linalg.norm(sizes/2))
self._collision_box_vertices_offset = np.array(self._collision_box_vertices_offset)
self._collision_box_hdiags = np.array(self._collision_box_hdiags)
self._collision_proj_axes = np.zeros((3, 15))
self._box_vertices_offset = np.ones([8, 3])
self._box_transform = np.eye(4)
def wait_for_franka_interface(self, timeout=None):
"""
Blocks execution until franka-interface gives ready signal.
Parameters
----------
timeout : :obj:`float`
Timeout in seconds to wait for franka-interface.
"""
timeout = FC.DEFAULT_FRANKA_INTERFACE_TIMEOUT if timeout is None else timeout
t_start = time()
while time() - t_start < timeout:
franka_interface_status = self._get_current_franka_interface_status().franka_interface_status
if franka_interface_status.is_ready:
return
sleep(1e-2)
raise FrankaArmCommException('FrankaInterface status not ready for {}s'.format(
FC.DEFAULT_FRANKA_INTERFACE_TIMEOUT))
def wait_for_skill(self):
"""
Blocks execution until skill is done.
"""
while not self.is_skill_done():
continue
def wait_for_gripper(self):
"""
Blocks execution until gripper is done.
"""
if self._last_gripper_command == "Grasp":
done = self._gripper_grasp_client.wait_for_result()
elif self._last_gripper_command == "Homing":
done = self._gripper_homing_client.wait_for_result()
elif self._last_gripper_command == "Stop":
done = self._gripper_stop_client.wait_for_result()
elif self._last_gripper_command == "Move":
done = self._gripper_move_client.wait_for_result()
sleep(2)
def is_skill_done(self, ignore_errors=True):
"""
Checks whether skill is done.
Parameters
----------
ignore_errors : :obj:`bool`
Flag of whether to ignore errors.
Returns
-------
:obj:`bool`
Flag of whether the skill is done.
"""
if not self._in_skill:
return True
franka_interface_status = self._get_current_franka_interface_status().franka_interface_status
e = None
if rospy.is_shutdown():
e = RuntimeError('rospy is down!')
elif franka_interface_status.error_description:
e = FrankaArmException(franka_interface_status.error_description)
elif not franka_interface_status.is_ready:
e = FrankaArmFrankaInterfaceNotReadyException()
if e is not None:
if ignore_errors:
self.wait_for_franka_interface()
else:
raise e
done = self._client.wait_for_result(rospy.Duration.from_sec(
FC.ACTION_WAIT_LOOP_TIME))
if done:
self._in_skill = False
return done
def stop_skill(self):
"""
Stops the current skill.
"""
if self._connected and self._in_skill:
self._client.cancel_goal()
self.wait_for_skill()
def _sigint_handler_gen(self):
def sigint_handler(sig, frame):
if self._connected and self._in_skill:
self._client.cancel_goal()
sys.exit(0)
return sigint_handler
def _send_goal(self, goal, cb, block=True, ignore_errors=True):
"""
Sends the goal to the Franka Interface ROS Action Server
Parameters
----------
goal : :obj:`ExecuteSkillGoal`
Skill with parameters that are sent to franka-interface.
cb : :obj:`function`
Function that takes in ExecuteSkillFeedback during skill execution.
block : :obj:`bool`
Flag that determines whether to block until skill is finished.
ignore_errors : :obj:`bool`
Flag of whether to ignore errors.
Returns
-------
:obj:`ExecuteSkillResult`
Skill Results that are sent from franka-interface.
Raises
------
FrankaArmCommException if a timeout is reached
FrankaArmException if franka-interface gives an error
FrankaArmFrankaInterfaceNotReadyException if franka-interface is not ready
"""
if self._offline:
logging.warn('In offline mode, FrankaArm will not execute real robot commands.')
return
if not self.is_skill_done():
raise ValueError('Cannot send another command when the previous skill is active!')
self._in_skill = True
self._client.send_goal(goal, feedback_cb=cb)
if not block:
return None
self.wait_for_skill()
return self._client.get_result()
"""
Controls
"""
def goto_pose(self,
tool_pose,
duration=3,
use_impedance=True,
dynamic=False,
buffer_time=FC.DEFAULT_TERM_BUFFER_TIME,
force_thresholds=None,
torque_thresholds=None,
cartesian_impedances=None,
joint_impedances=None,
block=True,
ignore_errors=True,
ignore_virtual_walls=False,
skill_desc='GoToPose'):
"""
Commands Arm to the given pose via min jerk interpolation
Parameters
----------
tool_pose : :obj:`autolab_core.RigidTransform`
End-effector pose in tool frame
duration : :obj:`float`
How much time this robot motion should take.
use_impedance : :obj:`bool`
Function uses our impedance controller by default.
If False, uses the Franka cartesian controller.
dynamic : :obj:`bool`
Flag that states whether the skill is dynamic.
If True, it will use our joint impedance controller
and sensor values.
buffer_time : :obj:`float`
How much extra time the termination handler will wait
before stopping the skill after duration has passed.
force_thresholds : :obj:`list`
List of 6 floats corresponding to force limits on
translation (xyz) and rotation about (xyz) axes.
Default is None. If None then will not stop on contact.
torque_thresholds : :obj:`list`
List of 7 floats corresponding to torque limits on each joint.
Default is None. If None then will not stop on contact.
cartesian_impedances : :obj:`list`
List of 6 floats corresponding to impedances on translation
(xyz) and rotation about (xyz) axes. Default is None.
If None then will use default impedances.
joint_impedances : :obj:`list`
List of 7 floats corresponding to impedances on each joint.
This is used when use_impedance is False. Default is None.
If None then will use default impedances.
block : :obj:`bool`
Function blocks by default. If False, the function becomes
asynchronous and can be preempted.
ignore_errors : :obj:`bool`
Function ignores errors by default. If False, errors and
some exceptions can be thrown.
ignore_virtual_walls : :obj:`bool`
Function checks for collisions with virtual walls by default.
If False, the robot no longer checks, which may be dangerous.
skill_desc : :obj:`str`
Skill description to use for logging on the Control PC.
Raises
------
ValueError: If tool_pose does not have from_frame=franka_tool and
to_frame=world or tool_pose is outside the virtual walls.
"""
if dynamic:
skill = Skill(SkillType.ImpedanceControlSkill,
TrajectoryGeneratorType.PassThroughPoseTrajectoryGenerator,
feedback_controller_type=FeedbackControllerType.CartesianImpedanceFeedbackController,
termination_handler_type=TerminationHandlerType.TimeTerminationHandler,
skill_desc=skill_desc)
use_impedance=True
block = False
else:
if use_impedance:
skill = Skill(SkillType.ImpedanceControlSkill,
TrajectoryGeneratorType.MinJerkPoseTrajectoryGenerator,
feedback_controller_type=FeedbackControllerType.CartesianImpedanceFeedbackController,
termination_handler_type=TerminationHandlerType.FinalPoseTerminationHandler,
skill_desc=skill_desc)
else:
skill = Skill(SkillType.CartesianPoseSkill,
TrajectoryGeneratorType.MinJerkPoseTrajectoryGenerator,
feedback_controller_type=FeedbackControllerType.SetInternalImpedanceFeedbackController,
termination_handler_type=TerminationHandlerType.FinalPoseTerminationHandler,
skill_desc=skill_desc)
if tool_pose.from_frame != 'franka_tool' or tool_pose.to_frame != 'world':
raise ValueError('pose has invalid frame names! Make sure pose has \
from_frame=franka_tool and to_frame=world')
tool_base_pose = tool_pose * self._tool_delta_pose.inverse()
if not ignore_virtual_walls and np.any([
tool_base_pose.translation <= FC.WORKSPACE_WALLS[:, :3].min(axis=0),
tool_base_pose.translation >= FC.WORKSPACE_WALLS[:, :3].max(axis=0)]):
raise ValueError('Target pose is outside of workspace virtual walls!')
skill.add_initial_sensor_values(FC.EMPTY_SENSOR_VALUES)
skill.set_cartesian_impedances(use_impedance, cartesian_impedances, joint_impedances)
if not skill.check_for_contact_params(buffer_time, force_thresholds, torque_thresholds):
if dynamic:
skill.add_time_termination_params(buffer_time)
else:
skill.add_pose_threshold_params(buffer_time, FC.DEFAULT_POSE_THRESHOLDS)
skill.add_goal_pose(duration, tool_base_pose)
goal = skill.create_goal()
self._send_goal(goal,
cb=lambda x: skill.feedback_callback(x),
block=block,
ignore_errors=ignore_errors)
if dynamic:
sleep(FC.DYNAMIC_SKILL_WAIT_TIME)
def goto_pose_delta(self,
delta_tool_pose,
duration=3,
use_impedance=True,
buffer_time=FC.DEFAULT_TERM_BUFFER_TIME,
force_thresholds=None,
torque_thresholds=None,
cartesian_impedances=None,
joint_impedances=None,
block=True,
ignore_errors=True,
ignore_virtual_walls=False,
skill_desc='GoToPoseDelta'):
"""
Commands Arm to the given delta pose via min jerk interpolation
Parameters
----------
delta_tool_pose : :obj:`autolab_core.RigidTransform`
Delta pose in tool frame
duration : :obj:`float`
How much time this robot motion should take.
use_impedance : :obj:`bool`
Function uses our impedance controller by default.
If False, uses the Franka cartesian controller.
buffer_time : :obj:`float`
How much extra time the termination handler will wait
before stopping the skill after duration has passed.
force_thresholds : :obj:`list`
List of 6 floats corresponding to force limits on translation
(xyz) and rotation about (xyz) axes.
Default is None. If None then will not stop on contact.
torque_thresholds : :obj:`list`
List of 7 floats corresponding to torque limits on each joint.
Default is None. If None then will not stop on contact.
cartesian_impedances : :obj:`list`
List of 6 floats corresponding to impedances on translation
(xyz) and rotation about (xyz) axes. Default is None.
If None then will use default impedances.
joint_impedances : :obj:`list`
List of 7 floats corresponding to impedances on each joint.
This is used when use_impedance is False. Default is None.
If None then will use default impedances.
block : :obj:`bool`
Function blocks by default. If False, the function becomes
asynchronous and can be preempted.
ignore_errors : :obj:`bool`
Function ignores errors by default. If False, errors and
some exceptions can be thrown.
ignore_virtual_walls : :obj:`bool`
Function checks for collisions with virtual walls by default.
If False, the robot no longer checks, which may be dangerous.
skill_desc : :obj:`str`
Skill description to use for logging on the Control PC.
Raises:
ValueError: If delta_pose does not have from_frame=franka_tool and
to_frame=franka_tool or from_frame=world and to_frame=world.
"""
if use_impedance:
skill = Skill(SkillType.ImpedanceControlSkill,
TrajectoryGeneratorType.RelativeMinJerkPoseTrajectoryGenerator,
feedback_controller_type=FeedbackControllerType.CartesianImpedanceFeedbackController,
termination_handler_type=TerminationHandlerType.FinalPoseTerminationHandler,
skill_desc=skill_desc)
else:
skill = Skill(SkillType.CartesianPoseSkill,
TrajectoryGeneratorType.RelativeMinJerkPoseTrajectoryGenerator,
feedback_controller_type=FeedbackControllerType.SetInternalImpedanceFeedbackController,
termination_handler_type=TerminationHandlerType.FinalPoseTerminationHandler,
skill_desc=skill_desc)
skill.add_initial_sensor_values(FC.EMPTY_SENSOR_VALUES)
skill.set_cartesian_impedances(use_impedance, cartesian_impedances, joint_impedances)
if not skill.check_for_contact_params(buffer_time, force_thresholds, torque_thresholds):
skill.add_pose_threshold_params(buffer_time, FC.DEFAULT_POSE_THRESHOLDS)
if delta_tool_pose.from_frame == 'world' \
and delta_tool_pose.to_frame == 'world':
skill.add_goal_pose(duration, delta_tool_pose)
elif delta_tool_pose.from_frame == 'franka_tool' \
and delta_tool_pose.to_frame == 'franka_tool':
starting_pose = self.get_pose()
starting_tool_base_pose = starting_pose * self._tool_delta_pose.inverse()
final_goal_pose = starting_pose * delta_tool_pose
final_tool_base_pose = final_goal_pose * self._tool_delta_pose.inverse()
delta_tool_base_pose = starting_tool_base_pose.inverse() * final_tool_base_pose
starting_rotation = RigidTransform(
rotation=starting_pose.rotation,
from_frame='franka_tool', to_frame='franka_tool'
)
predicted_translation = RigidTransform(
translation=delta_tool_base_pose.translation,
from_frame='franka_tool', to_frame='franka_tool'
)
actual_translation = starting_rotation * predicted_translation
delta_tool_base_pose.translation = actual_translation.translation
if not ignore_virtual_walls and not self._offline:
if np.any([
final_tool_base_pose.translation <= FC.WORKSPACE_WALLS[:, :3].min(axis=0),
final_tool_base_pose.translation >= FC.WORKSPACE_WALLS[:, :3].max(axis=0)]):
raise ValueError('Target pose is outside of workspace virtual walls!')
skill.add_goal_pose(duration, delta_tool_base_pose)
else:
raise ValueError('delta_pose has invalid frame names! ' \
'Make sure delta_pose has from_frame=franka_tool ' \
'and to_frame=franka_tool or from_frame=world and \
to_frame=world')
goal = skill.create_goal()
self._send_goal(goal,
cb=lambda x: skill.feedback_callback(x),
block=block,
ignore_errors=ignore_errors)
def goto_joints(self,
joints,
duration=5,
use_impedance=False,
dynamic=False,
buffer_time=FC.DEFAULT_TERM_BUFFER_TIME,
force_thresholds=None,
torque_thresholds=None,
cartesian_impedances=None,
joint_impedances=None,
k_gains=None,
d_gains=None,
block=True,
ignore_errors=True,
ignore_virtual_walls=False,
skill_desc='GoToJoints'):
"""
Commands Arm to the given joint configuration
Parameters
----------
joints : :obj:`list`
A list of 7 numbers that correspond to joint angles in radians.
duration : :obj:`float`
How much time this robot motion should take.
use_impedance : :obj:`bool`
Function uses the Franka joint impedance controller by default.
If True, uses our joint impedance controller.
dynamic : :obj:`bool`
Flag that states whether the skill is dynamic. If True, it
will use our joint impedance controller and sensor values.
buffer_time : :obj:`float`
How much extra time the termination handler will wait
before stopping the skill after duration has passed.
force_thresholds : :obj:`list`
List of 6 floats corresponding to force limits on translation
(xyz) and rotation about (xyz) axes. Default is None.
If None then will not stop on contact.
torque_thresholds : :obj:`list`
List of 7 floats corresponding to torque limits on each joint.
Default is None. If None then will not stop on contact.
cartesian_impedances : :obj:`list`
List of 6 floats corresponding to impedances on translation
(xyz) and rotation about (xyz) axes. Default is None. If None
then will use default impedances.
joint_impedances : :obj:`list`
List of 7 floats corresponding to impedances on each joint.
This is used when use_impedance is False. Default is None.
If None then will use default impedances.
k_gains : :obj:`list`
List of 7 floats corresponding to the k_gains on each joint for
our impedance controller. This is used when use_impedance is
True. Default is None. If None then will use default k_gains.
d_gains : :obj:`list`
List of 7 floats corresponding to the d_gains on each joint for
our impedance controller. This is used when use_impedance is
True. Default is None. If None then will use default d_gains.
block : :obj:`bool`
Function blocks by default. If False, the function becomes
asynchronous and can be preempted.
ignore_errors : :obj:`bool`
Function ignores errors by default. If False, errors and some
exceptions can be thrown.
ignore_virtual_walls : :obj:`bool`
Function checks for collisions with virtual walls by default.
If False, the robot no longer checks, which may be dangerous.
skill_desc : :obj:`str`
Skill description to use for logging on the Control PC.
Raises:
ValueError: If is_joints_reachable(joints) returns False
"""
joints = np.array(joints).tolist()
if dynamic:
skill = Skill(SkillType.ImpedanceControlSkill,
TrajectoryGeneratorType.PassThroughJointTrajectoryGenerator,
feedback_controller_type=FeedbackControllerType.JointImpedanceFeedbackController,
termination_handler_type=TerminationHandlerType.TimeTerminationHandler,
skill_desc=skill_desc)
use_impedance=True
block = False
else:
if use_impedance:
skill = Skill(SkillType.ImpedanceControlSkill,
TrajectoryGeneratorType.MinJerkJointTrajectoryGenerator,
feedback_controller_type=FeedbackControllerType.JointImpedanceFeedbackController,
termination_handler_type=TerminationHandlerType.FinalJointTerminationHandler,
skill_desc=skill_desc)
else:
skill = Skill(SkillType.JointPositionSkill,
TrajectoryGeneratorType.MinJerkJointTrajectoryGenerator,
feedback_controller_type=FeedbackControllerType.SetInternalImpedanceFeedbackController,
termination_handler_type=TerminationHandlerType.FinalJointTerminationHandler,
skill_desc=skill_desc)
if not self.is_joints_reachable(joints):
raise ValueError('Joints not reachable!')
if not ignore_virtual_walls and self.is_joints_in_collision_with_boxes(joints):
raise ValueError('Target joints in collision with virtual walls!')
skill.add_initial_sensor_values(FC.EMPTY_SENSOR_VALUES)
skill.set_joint_impedances(use_impedance, cartesian_impedances, joint_impedances, k_gains, d_gains)
if not skill.check_for_contact_params(buffer_time, force_thresholds, torque_thresholds):
if dynamic:
skill.add_time_termination_params(buffer_time)
else:
skill.add_joint_threshold_params(buffer_time, FC.DEFAULT_JOINT_THRESHOLDS)
skill.add_goal_joints(duration, joints)
goal = skill.create_goal()
self._send_goal(goal,
cb=lambda x: skill.feedback_callback(x),
block=block,
ignore_errors=ignore_errors)
if dynamic:
sleep(FC.DYNAMIC_SKILL_WAIT_TIME)
def execute_cartesian_velocities(self,
cartesian_velocities,
cartesian_accelerations,
duration=5,
dynamic=False,
buffer_time=FC.DEFAULT_TERM_BUFFER_TIME,
force_thresholds=None,
torque_thresholds=None,
cartesian_impedances=None,
joint_impedances=None,
block=True,
ignore_errors=True,
ignore_virtual_walls=False,
skill_desc='ExecutecartesianVelocities'):
"""
Commands Arm to execute cartesian velocities
Parameters
----------
cartesian_velocities : :obj:`list`
A list of 6 numbers that correspond to cartesian velocities in m/s and rad/s.
cartesian_accelerations : :obj:`list`
A list of 6 numbers that correspond to cartesian accelerations in m/s^2 and rad/s^2.
duration : :obj:`float`
How much time this robot motion should take.
dynamic : :obj:`bool`
Flag that states whether the skill is dynamic. If True, it
will use our cartesian impedance controller and sensor values.
buffer_time : :obj:`float`
How much extra time the termination handler will wait
before stopping the skill after duration has passed.
force_thresholds : :obj:`list`
List of 6 floats corresponding to force limits on translation
(xyz) and rotation about (xyz) axes. Default is None.
If None then will not stop on contact.
torque_thresholds : :obj:`list`
List of 7 floats corresponding to torque limits on each joint.
Default is None. If None then will not stop on contact.
cartesian_impedances : :obj:`list`
List of 6 floats corresponding to impedances on translation
(xyz) and rotation about (xyz) axes. Default is None. If None
then will use default impedances.
joint_impedances : :obj:`list`
List of 7 floats corresponding to impedances on each joint.
This is used when use_impedance is False. Default is None.
If None then will use default impedances.
block : :obj:`bool`
Function blocks by default. If False, the function becomes
asynchronous and can be preempted.
ignore_errors : :obj:`bool`
Function ignores errors by default. If False, errors and some
exceptions can be thrown.
ignore_virtual_walls : :obj:`bool`
Function checks for collisions with virtual walls by default.
If False, the robot no longer checks, which may be dangerous.
skill_desc : :obj:`str`
Skill description to use for logging on the Control PC.
Raises:
ValueError: If is_cartesians_reachable(cartesians) returns False
"""
cartesian_velocities = np.array(cartesian_velocities).tolist()
cartesian_accelerations = np.array(cartesian_accelerations).tolist()
if dynamic:
skill = Skill(SkillType.CartesianVelocitySkill,
TrajectoryGeneratorType.PassThroughCartesianVelocityTrajectoryGenerator,
feedback_controller_type=FeedbackControllerType.SetInternalImpedanceFeedbackController,
termination_handler_type=TerminationHandlerType.TimeTerminationHandler,
skill_desc=skill_desc)
block = False
else:
print("Unimplemented Cartesian Velocity Skill")
skill.add_initial_sensor_values(FC.EMPTY_SENSOR_VALUES)
skill.set_cartesian_impedances(False, cartesian_impedances, cartesian_impedances)
if not skill.check_for_contact_params(buffer_time, force_thresholds, torque_thresholds):
if dynamic:
skill.add_time_termination_params(buffer_time)
skill.add_goal_cartesian_velocities(duration, cartesian_velocities, cartesian_accelerations)
goal = skill.create_goal()
self._send_goal(goal,
cb=lambda x: skill.feedback_callback(x),
block=block,
ignore_errors=ignore_errors)
if dynamic:
sleep(FC.DYNAMIC_SKILL_WAIT_TIME)
def execute_joint_velocities(self,
joint_velocities,
joint_accelerations,
duration=5,
dynamic=False,
buffer_time=FC.DEFAULT_TERM_BUFFER_TIME,
force_thresholds=None,
torque_thresholds=None,
cartesian_impedances=None,
joint_impedances=None,
block=True,
ignore_errors=True,
ignore_virtual_walls=False,
skill_desc='ExecuteJointVelocities'):
"""
Commands Arm to execute joint velocities
Parameters
----------
joint_velocities : :obj:`list`
A list of 7 numbers that correspond to joint velocities in radians/second.
joint_accelerations : :obj:`list`
A list of 7 numbers that correspond to joint accelerations in radians/second^2.
duration : :obj:`float`
How much time this robot motion should take.
dynamic : :obj:`bool`
Flag that states whether the skill is dynamic. If True, it
will use our joint impedance controller and sensor values.
buffer_time : :obj:`float`
How much extra time the termination handler will wait
before stopping the skill after duration has passed.
force_thresholds : :obj:`list`
List of 6 floats corresponding to force limits on translation
(xyz) and rotation about (xyz) axes. Default is None.
If None then will not stop on contact.
torque_thresholds : :obj:`list`
List of 7 floats corresponding to torque limits on each joint.
Default is None. If None then will not stop on contact.
cartesian_impedances : :obj:`list`
List of 6 floats corresponding to impedances on translation
(xyz) and rotation about (xyz) axes. Default is None. If None
then will use default impedances.
joint_impedances : :obj:`list`
List of 7 floats corresponding to impedances on each joint.
This is used when use_impedance is False. Default is None.
If None then will use default impedances.
block : :obj:`bool`
Function blocks by default. If False, the function becomes
asynchronous and can be preempted.
ignore_errors : :obj:`bool`
Function ignores errors by default. If False, errors and some
exceptions can be thrown.
ignore_virtual_walls : :obj:`bool`
Function checks for collisions with virtual walls by default.
If False, the robot no longer checks, which may be dangerous.
skill_desc : :obj:`str`
Skill description to use for logging on the Control PC.
Raises:
ValueError: If is_joints_reachable(joints) returns False
"""
joint_velocities = np.array(joint_velocities).tolist()
joint_accelerations = np.array(joint_accelerations).tolist()
if dynamic:
skill = Skill(SkillType.JointVelocitySkill,
TrajectoryGeneratorType.PassThroughJointVelocityTrajectoryGenerator,
feedback_controller_type=FeedbackControllerType.SetInternalImpedanceFeedbackController,
termination_handler_type=TerminationHandlerType.TimeTerminationHandler,
skill_desc=skill_desc)
block = False
else:
print("Unimplemented Joint Velocity Skill")
skill.add_initial_sensor_values(FC.EMPTY_SENSOR_VALUES)
skill.set_joint_impedances(False, cartesian_impedances, joint_impedances, None, None)
if not skill.check_for_contact_params(buffer_time, force_thresholds, torque_thresholds):
if dynamic:
skill.add_time_termination_params(buffer_time)
else:
print("Unimplemented Joint Velocity Skill")
skill.add_goal_joint_velocities(duration, joint_velocities, joint_accelerations)
goal = skill.create_goal()
self._send_goal(goal,
cb=lambda x: skill.feedback_callback(x),
block=block,
ignore_errors=ignore_errors)
if dynamic:
sleep(FC.DYNAMIC_SKILL_WAIT_TIME)
def execute_joint_torques(self,
joint_torques,
selection=[0,0,0,0,0,0,0],
remove_gravity=[0,0,0,0,0,0,0],
duration=5,
dynamic=False,
buffer_time=FC.DEFAULT_TERM_BUFFER_TIME,
force_thresholds=None,
torque_thresholds=None,
k_gains=None,
d_gains=None,
block=True,
ignore_errors=True,
ignore_virtual_walls=False,
skill_desc='ExecuteJointTorques'):
"""
Commands Arm to execute joint torques
Parameters
----------
joint_torques : :obj:`list`
A list of 7 numbers that correspond to joint torques in radians/second.
selection : :obj:`list`
A list of 7 numbers that indicate whether to use the joint torques passed in or not.
If 1 is passed in for a specific joint, the robot will use the joint torque for that joint.
remove_gravity : :obj:`list`
A list of 7 numbers that indicate whether to remove gravity from that joint or not.
If 1 is passed in for a specific joint, the robot will subtract gravity for that joint.
duration : :obj:`float`
How much time this robot motion should take.
dynamic : :obj:`bool`
Flag that states whether the skill is dynamic. If True, it
will use our joint impedance controller and sensor values.
buffer_time : :obj:`float`
How much extra time the termination handler will wait
before stopping the skill after duration has passed.
force_thresholds : :obj:`list`
List of 6 floats corresponding to force limits on translation
(xyz) and rotation about (xyz) axes. Default is None.
If None then will not stop on contact.
torque_thresholds : :obj:`list`
List of 7 floats corresponding to torque limits on each joint.
Default is None. If None then will not stop on contact.
k_gains : :obj:`list`
List of 7 floats corresponding to the k_gains on each joint for
our impedance controller. Default is None. If None then will use
default k_gains.
d_gains : :obj:`list`
List of 7 floats corresponding to the d_gains on each joint for
our impedance controller. Default is None. If None then will use
default d_gains.
block : :obj:`bool`
Function blocks by default. If False, the function becomes
asynchronous and can be preempted.
ignore_errors : :obj:`bool`
Function ignores errors by default. If False, errors and some
exceptions can be thrown.
ignore_virtual_walls : :obj:`bool`
Function checks for collisions with virtual walls by default.
If False, the robot no longer checks, which may be dangerous.
skill_desc : :obj:`str`
Skill description to use for logging on the Control PC.
Raises:
ValueError: If is_joints_reachable(joints) returns False
"""
joint_torques = np.array(joint_torques).tolist()
selection = np.array(selection).tolist()
if dynamic:
block = False
else:
block = True
if k_gains is None:
k_gains = FC.DEFAULT_K_GAINS
else:
k_gains = np.array(k_gains).tolist()
if d_gains is None:
d_gains = FC.DEFAULT_D_GAINS
else:
d_gains = np.array(d_gains).tolist()
skill = Skill(SkillType.ImpedanceControlSkill,
TrajectoryGeneratorType.StayInInitialJointsTrajectoryGenerator,
feedback_controller_type=FeedbackControllerType.PassThroughJointTorqueFeedbackController,
termination_handler_type=TerminationHandlerType.TimeTerminationHandler,
skill_desc=skill_desc)
skill.add_initial_sensor_values(FC.EMPTY_SENSOR_VALUES)
skill.add_run_time(duration)
if not skill.check_for_contact_params(buffer_time, force_thresholds, torque_thresholds):
skill.add_time_termination_params(buffer_time)
skill.add_joint_torques(joint_torques, selection, remove_gravity, k_gains, d_gains)
goal = skill.create_goal()
self._send_goal(goal,
cb=lambda x: skill.feedback_callback(x),
block=block,
ignore_errors=ignore_errors)
if dynamic:
sleep(FC.DYNAMIC_SKILL_WAIT_TIME)
def execute_joint_dmp(self,
joint_dmp_info,
duration,
initial_sensor_values=None,
use_impedance=False,
buffer_time=FC.DEFAULT_TERM_BUFFER_TIME,
force_thresholds=None,
torque_thresholds=None,
cartesian_impedances=None,
joint_impedances=None,
k_gains=None,
d_gains=None,
block=True,
ignore_errors=True,
skill_desc='JointDmp'):
"""
Commands Arm to execute a given joint dmp
Parameters
----------
joint_dmp_info : :obj:`dict`
Contains all the parameters of a joint DMP
(tau, alpha, beta, num_basis, num_sensors, mu, h, and weights)
duration : :obj:`float`
How much time this robot motion should take.
initial_sensor_values : :obj:`list`
List of initial sensor values. If None it will default to ones.
use_impedance : :obj:`bool`
Function uses the Franka joint impedance controller by default.
If True, uses our joint impedance controller.
buffer_time : :obj:`float`
How much extra time the termination handler will wait
before stopping the skill after duration has passed.
force_thresholds : :obj:`list`
List of 6 floats corresponding to force limits on translation
(xyz) and rotation about (xyz) axes. Default is None.
If None then will not stop on contact.
torque_thresholds : :obj:`list`
List of 7 floats corresponding to torque limits on each joint.
Default is None. If None then will not stop on contact.
cartesian_impedances : :obj:`list`
List of 6 floats corresponding to impedances on translation
(xyz) and rotation about (xyz) axes. Default is None.
If None then will use default impedances.
joint_impedances : :obj:`list`
List of 7 floats corresponding to impedances on each joint.
This is used when use_impedance is False. Default is None.
If None then will use default impedances.
k_gains : :obj:`list`
List of 7 floats corresponding to the k_gains on each joint
for our impedance controller. This is used when use_impedance is
True. Default is None. If None then will use default k_gains.
d_gains : :obj:`list`
List of 7 floats corresponding to the d_gains on each joint
for our impedance controller. This is used when use_impedance is
True. Default is None. If None then will use default d_gains.
block : :obj:`bool`
Function blocks by default. If False, the function
becomes asynchronous and can be preempted.
ignore_errors : :obj:`bool`
Function ignores errors by default.
If False, errors and some exceptions can be thrown.
skill_desc : :obj:`str`
Skill description to use for logging on control-pc.
"""
if use_impedance:
skill = Skill(SkillType.ImpedanceControlSkill,
TrajectoryGeneratorType.JointDmpTrajectoryGenerator,
feedback_controller_type=FeedbackControllerType.JointImpedanceFeedbackController,
termination_handler_type=TerminationHandlerType.FinalJointTerminationHandler,
skill_desc=skill_desc)
else:
skill = Skill(SkillType.JointPositionSkill,
TrajectoryGeneratorType.JointDmpTrajectoryGenerator,
feedback_controller_type=FeedbackControllerType.SetInternalImpedanceFeedbackController,
termination_handler_type=TerminationHandlerType.FinalJointTerminationHandler,
skill_desc=skill_desc)
if initial_sensor_values is None:
initial_sensor_values = np.ones(joint_dmp_info['num_sensors']).tolist()
skill.add_initial_sensor_values(initial_sensor_values) # sensor values
skill.add_joint_dmp_params(duration, joint_dmp_info, initial_sensor_values)
skill.set_joint_impedances(use_impedance, cartesian_impedances, joint_impedances, k_gains, d_gains)
if not skill.check_for_contact_params(buffer_time, force_thresholds, torque_thresholds):
skill.add_time_termination_params(buffer_time)
goal = skill.create_goal()
self._send_goal(goal,
cb=lambda x: skill.feedback_callback(x),
block=block,
ignore_errors=ignore_errors)
def execute_pose_dmp(self,
pose_dmp_info,
duration,
use_goal_formulation=False,
initial_sensor_values=None,
orientation_only = False,
position_only = False,
ee_frame = False,
use_impedance=True,
buffer_time=FC.DEFAULT_TERM_BUFFER_TIME,
force_thresholds=None,
torque_thresholds=None,
cartesian_impedances=None,
joint_impedances=None,
block=True,
ignore_errors=True,
skill_desc='PoseDmp'):
"""
Commands Arm to execute a given pose dmp
Parameters
----------
pose_dmp_info : :obj:`dict`
Contains all the parameters of a pose DMP
(tau, alpha, beta, num_basis, num_sensors, mu, h, and weights)
duration : :obj:`float`
How much time this robot motion should take.
use_goal_formulation : :obj:`bool`
Flag that represents whether to use the explicit goal pose
dmp formulation.
initial_sensor_values : :obj:`list`
List of initial sensor values. If None it will default to ones.
orientation_only : :obj:`bool`
Flag that represents if the dmp weights are to generate a dmp
only for orientation.
position_only : :obj:`bool`
Flag that represents if the dmp weights are to generate a dmp
only for position.
ee_frame : :obj:`bool`
Flag that indicates whether the dmp is in terms of the
end-effector frame.
use_impedance : :obj:`bool`
Function uses our impedance controller by default. If False,
uses the Franka cartesian controller.
buffer_time : :obj:`float`
How much extra time the termination handler will wait
before stopping the skill after duration has passed.
force_thresholds : :obj:`list`
List of 6 floats corresponding to force limits on translation
(xyz) and rotation about (xyz) axes. Default is None.
If None then will not stop on contact.
torque_thresholds : :obj:`list`
List of 7 floats corresponding to torque limits on each joint.
Default is None. If None then will not stop on contact.
cartesian_impedances : :obj:`list`
List of 6 floats corresponding to impedances on translation
(xyz) and rotation about (xyz) axes. Default is None.
If None then will use default impedances.
joint_impedances : :obj:`list`
List of 7 floats corresponding to impedances on each joint.
This is used when use_impedance is False. Default is None.
If None then will use default impedances.
block : :obj:`bool`
Function blocks by default. If False, the function becomes
asynchronous and can be preempted.
ignore_errors : :obj:`bool`
Function ignores errors by default. If False, errors and
some exceptions can be thrown.
skill_desc : :obj:`str`
Skill description to use for logging on control-pc.
"""
if use_impedance:
if use_goal_formulation:
skill = Skill(SkillType.ImpedanceControlSkill,
TrajectoryGeneratorType.GoalPoseDmpTrajectoryGenerator,
feedback_controller_type=FeedbackControllerType.CartesianImpedanceFeedbackController,
termination_handler_type=TerminationHandlerType.TimeTerminationHandler,
skill_desc=skill_desc)
else:
skill = Skill(SkillType.ImpedanceControlSkill,
TrajectoryGeneratorType.PoseDmpTrajectoryGenerator,
feedback_controller_type=FeedbackControllerType.CartesianImpedanceFeedbackController,
termination_handler_type=TerminationHandlerType.TimeTerminationHandler,
skill_desc=skill_desc)
else:
if use_goal_formulation:
skill = Skill(SkillType.CartesianPoseSkill,
TrajectoryGeneratorType.GoalPoseDmpTrajectoryGenerator,
feedback_controller_type=FeedbackControllerType.SetInternalImpedanceFeedbackController,
termination_handler_type=TerminationHandlerType.TimeTerminationHandler,
skill_desc=skill_desc)
else:
skill = Skill(SkillType.CartesianPoseSkill,
TrajectoryGeneratorType.PoseDmpTrajectoryGenerator,
feedback_controller_type=FeedbackControllerType.SetInternalImpedanceFeedbackController,
termination_handler_type=TerminationHandlerType.TimeTerminationHandler,
skill_desc=skill_desc)
if initial_sensor_values is None:
if orientation_only or position_only:
initial_sensor_values = np.ones(3*pose_dmp_info['num_sensors']).tolist()
else:
initial_sensor_values = np.ones(6*pose_dmp_info['num_sensors']).tolist()
skill.add_initial_sensor_values(initial_sensor_values) # sensor values
skill.add_pose_dmp_params(orientation_only, position_only, ee_frame, duration, pose_dmp_info, initial_sensor_values)
skill.set_cartesian_impedances(use_impedance, cartesian_impedances, joint_impedances)
if not skill.check_for_contact_params(buffer_time, force_thresholds, torque_thresholds):
skill.add_time_termination_params(buffer_time)
goal = skill.create_goal()
self._send_goal(goal,
cb=lambda x: skill.feedback_callback(x),
block=block,
ignore_errors=ignore_errors)
def execute_quaternion_pose_dmp(self,
position_dmp_info,
quat_dmp_info,
duration,
goal_quat,
goal_quat_time,
use_goal_formulation=False,
initial_sensor_values=None,
ee_frame=False,
use_impedance=True,
buffer_time=FC.DEFAULT_TERM_BUFFER_TIME,
force_thresholds=None,
torque_thresholds=None,
cartesian_impedances=None,
joint_impedances=None,
block=True,
ignore_errors=True,
skill_desc='QuaternionPoseDmp'):
'''Commands Arm to execute a given quaternion pose dmp
Args:
position_dmp_info : :obj:`dict`
Contains all the parameters of a position DMP
(tau, alpha, beta, num_basis, num_sensors, mu, h, and weights)
quat_dmp_info : :obj:`dict`
Contains all the parameters of a quaternion DMP
(tau, alpha, beta, num_basis, num_sensors, mu, h, and weights)
duration : :obj:`float`
How much time this robot motion should take.
goal_quat : :obj:`list`
List of 4 floats that represents the quaternion goal to reach.
This assumes an explicit goal formulation for the quaternion DMPs.
goal_quat_time : :obj:`float`
Time to reach goal for quaternion DMPs.
use_goal_formulation : :obj:`bool`
Flag that represents whether to use the explicit goal pose dmp formulation.
initial_sensor_values : :obj:`list`
List of initial sensor values. If None it will default to ones.
ee_frame : :obj:`bool`
Flag that indicates whether the dmp is in terms of the
end-effector frame.
use_impedance : :obj:`bool`
Function uses our impedance controller by default. If False,
uses the Franka cartesian controller.
buffer_time : :obj:`float`
How much extra time the termination handler will wait
before stopping the skill after duration has passed.
force_thresholds : :obj:`list`
List of 6 floats corresponding to force limits on translation
(xyz) and rotation about (xyz) axes. Default is None.
If None then will not stop on contact.
torque_thresholds : :obj:`list`
List of 7 floats corresponding to torque limits on each joint.
Default is None. If None then will not stop on contact.
cartesian_impedances : :obj:`list`
List of 6 floats corresponding to impedances on translation
(xyz) and rotation about (xyz) axes. Default is None.
If None then will use default impedances.
joint_impedances : :obj:`list`
List of 7 floats corresponding to impedances on each joint.
This is used when use_impedance is False. Default is None.
If None then will use default impedances.
block : :obj:`bool`
Function blocks by default. If False, the function becomes
asynchronous and can be preempted.
ignore_errors : :obj:`bool`
Function ignores errors by default. If False, errors and some
exceptions can be thrown.
skill_desc : :obj:`str`
Skill description to use for logging on control-pc.
'''
if use_impedance:
skill = Skill(SkillType.ImpedanceControlSkill,
TrajectoryGeneratorType.QuaternionPoseDmpTrajectoryGenerator,
feedback_controller_type=FeedbackControllerType.CartesianImpedanceFeedbackController,
termination_handler_type=TerminationHandlerType.TimeTerminationHandler,
skill_desc=skill_desc)
else:
skill = Skill(SkillType.CartesianPoseSkill,
TrajectoryGeneratorType.QuaternionPoseDmpTrajectoryGenerator,
feedback_controller_type=FeedbackControllerType.SetInternalImpedanceFeedbackController,
termination_handler_type=TerminationHandlerType.TimeTerminationHandler,
skill_desc=skill_desc)
if initial_sensor_values is None:
initial_sensor_values = np.ones(3*position_dmp_info['num_sensors']).tolist()
skill.add_initial_sensor_values(initial_sensor_values) # sensor values
skill.add_quaternion_pose_dmp_params(ee_frame, duration, position_dmp_info, quat_dmp_info, initial_sensor_values, goal_quat, goal_quat_time)
skill.set_cartesian_impedances(use_impedance, cartesian_impedances, joint_impedances)
if not skill.check_for_contact_params(buffer_time, force_thresholds, torque_thresholds):
skill.add_time_termination_params(buffer_time)
goal = skill.create_goal()
self._send_goal(goal,
cb=lambda x: skill.feedback_callback(x),
block=block,
ignore_errors=ignore_errors)
def apply_effector_forces_torques(self,
run_duration,
acc_duration,
max_translation,
max_rotation,
forces=None,
torques=None,
buffer_time=FC.DEFAULT_TERM_BUFFER_TIME,
force_thresholds=None,
torque_thresholds=None,
block=True,
ignore_errors=True,
skill_desc='ForceTorque'):
"""
Applies the given end-effector forces and torques in N and Nm
Parameters
----------
run_duration : :obj:`float`
How much time this robot motion should take.
acc_duration : :obj:`float`
How long to acc/de-acc to achieve desired force.
max_translation : :obj:`float`
A float in the unit of meters. Max translation before the
robot deaccelerates.
max_rotation : :obj:`float`
A float in the unit of rad. Max rotation before the robot
deaccelerates.
forces : :obj:`list`
Optional (defaults to None). A list of 3 numbers that correspond
to end-effector forces in (xyz) directions.
torques : :obj:`list`
Optional (defaults to None). A list of 3 numbers that correspond
to end-effector torques in 3 rotational axes.
buffer_time : :obj:`float`
How much extra time the termination handler will wait
before stopping the skill after duration has passed.
force_thresholds : :obj:`list`
List of 6 floats corresponding to force limits on translation
(xyz) and rotation about (xyz) axes. Default is None.
If None then will not stop on contact.
torque_thresholds : :obj:`list`
List of 7 floats corresponding to torque limits on each joint.
Default is None. If None then will not stop on contact.
block : :obj:`bool`
Function blocks by default. If False, the function becomes
asynchronous and can be preempted.
ignore_errors : :obj:`bool`
Function ignores errors by default. If False, errors and some
exceptions can be thrown.
skill_desc : :obj:`str`
Skill description to use for logging on control-pc.
Raises:
ValueError if acc_duration > 0.5*run_duration, or if forces are
too large
"""
if acc_duration > 0.5 * run_duration:
raise ValueError(
'acc_duration must be smaller than half of run_duration!')
forces = [0, 0, 0] if forces is None else np.array(forces).tolist()
torques = [0, 0, 0] if torques is None else np.array(torques).tolist()
if np.linalg.norm(forces) * run_duration > FC.MAX_LIN_MOMENTUM:
raise ValueError('Linear momentum magnitude exceeds safety '
'threshold of {}'.format(FC.MAX_LIN_MOMENTUM))
if np.linalg.norm(torques) * run_duration > FC.MAX_ANG_MOMENTUM:
raise ValueError('Angular momentum magnitude exceeds safety '
'threshold of {}'.format(FC.MAX_ANG_MOMENTUM))
skill = Skill(SkillType.ForceTorqueSkill,
TrajectoryGeneratorType.ImpulseTrajectoryGenerator,
feedback_controller_type=FeedbackControllerType.PassThroughFeedbackController,
termination_handler_type=TerminationHandlerType.TimeTerminationHandler,
skill_desc=skill_desc)
skill.add_initial_sensor_values(FC.EMPTY_SENSOR_VALUES)
skill.add_impulse_params(run_duration, acc_duration, max_translation, max_rotation, forces, torques)
if not skill.check_for_contact_params(buffer_time, force_thresholds, torque_thresholds):
skill.add_time_termination_params(buffer_time)
goal = skill.create_goal()
self._send_goal(goal,
cb=lambda x: skill.feedback_callback(x),
block=block,
ignore_errors=ignore_errors)
def apply_effector_forces_along_axis(self,
run_duration,
acc_duration,
max_translation,
forces,
buffer_time=FC.DEFAULT_TERM_BUFFER_TIME,
force_thresholds=None,
torque_thresholds=None,
block=True,
ignore_errors=True,
skill_desc='ForcesAlongAxis'):
"""
Applies the given end-effector forces and torques in N and Nm
Parameters
----------
run_duration : :obj:`float`
How much time this robot motion should take.
acc_duration : :obj:`float`
How long to acc/de-acc to achieve desired force.
max_translation : :obj:`float`
A float in the unit of meters. Max translation before the
robot deaccelerates.
forces : :obj:`list`
Optional (defaults to None). A list of 3 numbers that correspond
to the magnitude of the end-effector forces and the axis.
buffer_time : :obj:`float`
How much extra time the termination handler will wait
before stopping the skill after duration has passed.
force_thresholds : :obj:`list`
List of 6 floats corresponding to force limits on translation
(xyz) and rotation about (xyz) axes. Default is None.
If None then will not stop on contact.
torque_thresholds : :obj:`list`
List of 7 floats corresponding to torque limits on each joint.
Default is None. If None then will not stop on contact.
block : :obj:`bool`
Function blocks by default. If False, the function becomes
asynchronous and can be preempted.
ignore_errors : :obj:`bool`
Function ignores errors by default. If False, errors and some
exceptions can be thrown.
skill_desc : :obj:`str`
Skill description to use for logging on control-pc.
Raises:
ValueError if acc_duration > 0.5*run_duration, or if forces are
too large
"""
if acc_duration > 0.5 * run_duration:
raise ValueError(
'acc_duration must be smaller than half of run_duration!')
if np.linalg.norm(forces) * run_duration > FC.MAX_LIN_MOMENTUM_CONSTRAINED:
raise ValueError('Linear momentum magnitude exceeds safety ' \
'threshold of {}'.format(FC.MAX_LIN_MOMENTUM_CONSTRAINED))
forces = np.array(forces)
force_axis = forces / np.linalg.norm(forces)
skill = Skill(SkillType.ForceTorqueSkill,
TrajectoryGeneratorType.ImpulseTrajectoryGenerator,
feedback_controller_type=FeedbackControllerType.ForceAxisImpedenceFeedbackController,
termination_handler_type=TerminationHandlerType.TimeTerminationHandler,
skill_desc=skill_desc)
skill.add_initial_sensor_values(FC.EMPTY_SENSOR_VALUES)
skill.add_impulse_params(run_duration, acc_duration, max_translation, 0, forces.tolist(), [0, 0, 0])
skill.add_force_axis_params(FC.DEFAULT_FORCE_AXIS_TRANSLATIONAL_STIFFNESS,
FC.DEFAULT_FORCE_AXIS_ROTATIONAL_STIFFNESS,
force_axis.tolist())
if not skill.check_for_contact_params(buffer_time, force_thresholds, torque_thresholds):
skill.add_time_termination_params(buffer_time)
goal = skill.create_goal()
self._send_goal(goal,
cb=lambda x: skill.feedback_callback(x),
block=block,
ignore_errors=ignore_errors)
def goto_gripper(self,
width,
grasp=False,
speed=0.04,
force=0.0,
epsilon_inner=0.08,
epsilon_outer=0.08,
block=True,
ignore_errors=True,
skill_desc='GoToGripper'):
"""
Commands gripper to goto a certain width, applying up to the given
(default is max) force if needed
Parameters
----------
width : :obj:`float`
Desired width of the gripper in the unit of meters.
grasp : :obj:`bool`
Flag that signals whether to grasp.
speed : :obj:`float`
Gripper operation speed in meters per sec.
force : :obj:`float`
Max gripper force to apply in N. Default to None,
which gives acceptable force.
epsilon_inner : :obj:`float`
Max tolerated deviation when the actual grasped
width is smaller than the commanded grasp width.
epsilon_outer : :obj:`float`
Max tolerated deviation when the actual grasped
width is larger than the commanded grasp width.
block : :obj:`bool`
Function blocks by default. If False, the function
becomes asynchronous and can be preempted.
ignore_errors : :obj:`bool`
Function ignores errors by default. If False, errors
and some exceptions can be thrown.
skill_desc : :obj:`str`
Skill description to use for logging on control-pc.
Raises:
ValueError: If width is less than 0 or greater than TODO(jacky)
the maximum gripper opening
"""
if width < FC.GRIPPER_WIDTH_MIN or width > FC.GRIPPER_WIDTH_MAX:
raise ValueError(
'gripper width must be within the gripper limits of ' \
'{} and {}'.format(FC.GRIPPER_WIDTH_MIN,FC.GRIPPER_WIDTH_MAX))
if self._old_gripper:
skill = Skill(SkillType.GripperSkill,
TrajectoryGeneratorType.GripperTrajectoryGenerator,
skill_desc=skill_desc)
skill.add_initial_sensor_values(FC.EMPTY_SENSOR_VALUES)
skill.add_gripper_params(grasp, width, speed, force)
goal = skill.create_goal()
self._send_goal(goal,
cb=lambda x: skill.feedback_callback(x),
block=block,
ignore_errors=ignore_errors)
# this is so the gripper state can be updated, which happens with a
# small lag
sleep(FC.GRIPPER_CMD_SLEEP_TIME)
else:
if grasp:
grasp_skill = GraspGoal()
grasp_skill.width = width
grasp_skill.speed = speed
grasp_skill.force = force
grasp_skill.epsilon.inner = epsilon_inner
grasp_skill.epsilon.outer = epsilon_outer
self._gripper_grasp_client.send_goal(grasp_skill)
else:
move_skill = MoveGoal()
move_skill.width = width
move_skill.speed = speed
self._gripper_move_client.send_goal(move_skill)
if block:
self.wait_for_gripper()
def selective_guidance_mode(self,
duration=5,
use_joints=False,
use_impedance=False,
use_ee_frame=False,
buffer_time=FC.DEFAULT_TERM_BUFFER_TIME,
force_thresholds=None,
torque_thresholds=None,
cartesian_impedances=None,
joint_impedances=None,
k_gains=None,
d_gains=None,
block=True,
ignore_errors=True,
ignore_virtual_walls=False,
skill_desc='SelectiveGuidance'):
"""
Commands the Arm to stay in its current position with selective impedances
that allow guidance in either certain joints or in cartesian pose.
Parameters
----------
duration : :obj:`float`
How much time this guidance should take
use_joints : :obj:`bool`
Function uses cartesian impedance controller by default.
If True, it uses joint impedance.
use_impedance : :obj:`bool`
Function uses the Franka joint impedance controller by default.
If True, uses our joint impedance controller.
use_ee_frame : :obj:`bool`
Function uses the end-effector cartesian feedback controller
only when use_impedance is True.
buffer_time : :obj:`float`
How much extra time the termination handler will wait
before stopping the skill after duration has passed.
force_thresholds : :obj:`list`
List of 6 floats corresponding to force limits on translation
(xyz) and rotation about (xyz) axes. Default is None.
If None then will not stop on contact.
torque_thresholds : :obj:`list`
List of 7 floats corresponding to torque limits on each joint.
Default is None. If None then will not stop on contact.
cartesian_impedances : :obj:`list`
List of 6 floats corresponding to impedances on translation
(xyz) and rotation about (xyz) axes. Default is None.
If None then will use default impedances.
joint_impedances : :obj:`list`
List of 7 floats corresponding to impedances on each joint.
This is used when use_impedance is False. Default is None.
If None then will use default impedances.
k_gains : :obj:`list`
List of 7 floats corresponding to the k_gains on each joint
for our impedance controller. This is used when use_impedance is
True. Default is None. If None then will use default k_gains.
d_gains : :obj:`list`
List of 7 floats corresponding to the d_gains on each joint
for our impedance controller. This is used when use_impedance is
True. Default is None. If None then will use default d_gains.
block : :obj:`bool`
Function blocks by default. If False, the function becomes
asynchronous and can be preempted.
ignore_errors : :obj:`bool`
Function ignores errors by default. If False, errors and some
exceptions can be thrown.
ignore_virtual_walls : :obj:`bool`
Function checks for collisions with virtual walls by default.
If False, the robot no longer checks, which may be dangerous.
skill_desc : :obj:`str`
Skill description to use for logging on control-pc.
"""
if use_joints:
if use_impedance:
skill = Skill(SkillType.ImpedanceControlSkill,
TrajectoryGeneratorType.StayInInitialJointsTrajectoryGenerator,
feedback_controller_type=FeedbackControllerType.JointImpedanceFeedbackController,
termination_handler_type=TerminationHandlerType.TimeTerminationHandler,
skill_desc=skill_desc)
else:
skill = Skill(SkillType.JointPositionSkill,
TrajectoryGeneratorType.StayInInitialJointsTrajectoryGenerator,
feedback_controller_type=FeedbackControllerType.SetInternalImpedanceFeedbackController,
termination_handler_type=TerminationHandlerType.TimeTerminationHandler,
skill_desc=skill_desc)
else:
if use_impedance:
if use_ee_frame:
skill = Skill(SkillType.ImpedanceControlSkill,
TrajectoryGeneratorType.StayInInitialPoseTrajectoryGenerator,
feedback_controller_type=FeedbackControllerType.EECartesianImpedanceFeedbackController,
termination_handler_type=TerminationHandlerType.TimeTerminationHandler,
skill_desc=skill_desc)
else:
skill = Skill(SkillType.ImpedanceControlSkill,
TrajectoryGeneratorType.StayInInitialPoseTrajectoryGenerator,
feedback_controller_type=FeedbackControllerType.CartesianImpedanceFeedbackController,
termination_handler_type=TerminationHandlerType.TimeTerminationHandler,
skill_desc=skill_desc)
else:
skill = Skill(SkillType.CartesianPoseSkill,
TrajectoryGeneratorType.StayInInitialPoseTrajectoryGenerator,
feedback_controller_type=FeedbackControllerType.SetInternalImpedanceFeedbackController,
termination_handler_type=TerminationHandlerType.TimeTerminationHandler,
skill_desc=skill_desc)
skill.add_initial_sensor_values(FC.EMPTY_SENSOR_VALUES)
skill.add_run_time(duration)
if use_joints:
skill.set_joint_impedances(use_impedance, cartesian_impedances, joint_impedances, k_gains, d_gains)
else:
skill.set_cartesian_impedances(use_impedance, cartesian_impedances, joint_impedances)
if not skill.check_for_contact_params(buffer_time, force_thresholds, torque_thresholds):
skill.add_time_termination_params(buffer_time)
goal = skill.create_goal()
self._send_goal(goal,
cb=lambda x: skill.feedback_callback(x),
block=block,
ignore_errors=ignore_errors)
def open_gripper(self, block=True, skill_desc='OpenGripper'):
"""
Opens gripper to maximum width
Parameters
----------
block : :obj:`bool`
Function blocks by default. If False, the function becomes
asynchronous and can be preempted.
skill_desc : :obj:`str`
Skill description to use for logging on control-pc.
"""
self.goto_gripper(FC.GRIPPER_WIDTH_MAX, block=block, skill_desc=skill_desc)
def close_gripper(self, grasp=True, block=True, skill_desc='CloseGripper'):
"""
Closes the gripper as much as possible
Parameters
----------
grasp : :obj:`bool`
Flag that signals whether to grasp.
block : :obj:`bool`
Function blocks by default. If False, the function becomes
asynchronous and can be preempted.
skill_desc : :obj:`str`
Skill description to use for logging on control-pc.
"""
self.goto_gripper(FC.GRIPPER_WIDTH_MIN, grasp=grasp,
force=FC.GRIPPER_MAX_FORCE if grasp else None,
block=block, skill_desc=skill_desc)
def home_gripper(self, block=True, skill_desc='HomeGripper'):
"""
Homes the gripper.
Parameters
----------
block : :obj:`bool`
Function blocks by default. If False, the function becomes
asynchronous and can be preempted.
skill_desc : :obj:`str`
Skill description to use for logging on control-pc.
"""
self._last_gripper_command = 'Homing'
homing_skill = HomingGoal()
self._gripper_homing_client.send_goal(homing_skill)
if block:
self.wait_for_gripper()
def stop_gripper(self, block=True, skill_desc='StopGripper'):
"""
Stops the gripper.
Parameters
----------
block : :obj:`bool`
Function blocks by default. If False, the function becomes
asynchronous and can be preempted.
skill_desc : :obj:`str`
Skill description to use for logging on control-pc.
"""
self._last_gripper_command = 'Stop'
stop_skill = StopGoal()
self._gripper_stop_client.send_goal(stop_skill)
if block:
self.wait_for_gripper()
def run_guide_mode(self, duration=10, block=True, skill_desc='GuideMode'):
"""
Runs guide mode which allows you to move the robot freely.
Parameters
----------
duration : :obj:`float`
How much time this guidance should take
block : :obj:`bool`
Function blocks by default. If False, the function becomes
asynchronous and can be preempted.
skill_desc : :obj:`str`
Skill description to use for logging on control-pc.
"""
self.apply_effector_forces_torques(duration, 0, 0, 0, block=block, skill_desc=skill_desc)
def run_dynamic_force_position(self,
duration=3,
buffer_time=FC.DEFAULT_TERM_BUFFER_TIME,
force_thresholds=None,
torque_thresholds=None,
position_kps_cart=FC.DEFAULT_TRANSLATIONAL_STIFFNESSES + FC.DEFAULT_ROTATIONAL_STIFFNESSES,
force_kps_cart=FC.DEFAULT_HFPC_FORCE_GAIN,
position_kps_joint=FC.DEFAULT_K_GAINS,
force_kps_joint=FC.DEFAULT_HFPC_FORCE_GAIN,
S=FC.DEFAULT_HFPC_S,
interpolate=False,
use_cartesian_gains=True,
ignore_errors=True,
ignore_virtual_walls=False,
skill_desc=''):
"""
Commands Arm to run dynamic hybrid force position control
Parameters
----------
duration : :obj:`float`
How much time this robot motion should take.
use_impedance : :obj:`bool`
Function uses our impedance controller by default.
If False, uses the Franka cartesian controller.
buffer_time : :obj:`float`
How much extra time the termination handler will wait
before stopping the skill after duration has passed.
force_thresholds : :obj:`list`
List of 6 floats corresponding to force limits on translation
(xyz) and rotation about (xyz) axes. Default is None.
If None then will not stop on contact.
torque_thresholds : :obj:`list`
List of 7 floats corresponding to torque limits on each joint.
Default is None. If None then will not stop on contact.
position_kp_cart : :obj:`list`
List of 6 floats corresponding to proportional gain used for
position errors in cartesian space.
force_kp_cart : :obj:`list`
List of 6 floats corresponding to proportional gain used for
force errors in cartesian space.
position_kp_joint : :obj:`list`
List of 7 floats corresponding to proportional gain used for
position errors in joint space.
force_kp_joint : :obj:`list`
List of 6 floats corresponding to proportional gain used for
force errors in joint space.
S : :obj:`list`
List of 6 numbers between 0 and 1 for the HFPC selection matrix.
interpolate : :obj:`bool`
Whether or not to perform linear interpolation in between way points.
use_cartesian_gains : :obj:`bool`
Whether to use cartesian gains or joint gains.
ignore_errors : :obj:`bool`
Function ignores errors by default. If False, errors and some
exceptions can be thrown.
ignore_virtual_walls : :obj:`bool`
Function checks for collisions with virtual walls by default.
If False, the robot no longer checks, which may be dangerous.
skill_desc : :obj:`str`
Skill description to use for logging on control-pc.
"""
if interpolate:
traj_gen = TrajectoryGeneratorType.LinearForcePositionTrajectoryGenerator
else:
traj_gen = TrajectoryGeneratorType.PassThroughForcePositionTrajectoryGenerator
skill = Skill(SkillType.ForceTorqueSkill,
traj_gen,
feedback_controller_type=FeedbackControllerType.ForcePositionFeedbackController,
termination_handler_type=TerminationHandlerType.TimeTerminationHandler,
skill_desc=skill_desc)
skill.add_initial_sensor_values(FC.EMPTY_SENSOR_VALUES)
skill.add_force_position_params(position_kps_cart, force_kps_cart, position_kps_joint, force_kps_joint, S, use_cartesian_gains)
skill.add_run_time(duration)
if not skill.check_for_contact_params(buffer_time, force_thresholds, torque_thresholds):
skill.add_time_termination_params(buffer_time)
goal = skill.create_goal()
self._send_goal(goal,
cb=lambda x: skill.feedback_callback(x),
block=False,
ignore_errors=ignore_errors)
sleep(FC.DYNAMIC_SKILL_WAIT_TIME)
"""
Reads
"""
def get_robot_state(self):
"""
Returns
-------
robot_state : :obj:`dict`
Dict that contains all of the robot's current
state information.
"""
return self._state_client.get_data()
def get_pose(self, include_tool_offset=True):
"""
Returns
-------
pose : :obj:`autolab_core.RigidTransform`
Current pose of the end-effector including the transform
to the end of the tool.
"""
tool_base_pose = self._state_client.get_pose()
if include_tool_offset:
tool_pose = tool_base_pose * self._tool_delta_pose
return tool_pose
else:
return tool_base_pose
def get_joints(self):
"""
Returns the current joint positions.
Returns
-------
joint_positions : :obj:`numpy.ndarray`
7 floats that represent each joint's position in radians.
"""
return self._state_client.get_joints()
def get_joint_torques(self):
"""
Returns the current joint torques.
Returns
-------
joint_torques : :obj:`numpy.ndarray`
7 floats that represent each joint's torque in Nm.
"""
return self._state_client.get_joint_torques()
def get_joint_velocities(self):
"""
Returns the current joint velocities.
Returns
-------
joint_velocities : :obj:`numpy.ndarray`
7 floats that represent each joint's velocity in rads/s.
"""
return self._state_client.get_joint_velocities()
def get_gripper_width(self):
"""
Returns the current gripper width.
Returns
-------
gripper_width : :obj:`float`
Robot gripper width in meters.
"""
if self._old_gripper:
return self._state_client.get_gripper_width()
else:
gripper_data = rospy.wait_for_message(self._gripper_joint_states_name, JointState)
return gripper_data.position[0] + gripper_data.position[1]
def get_gripper_is_grasped(self):
"""
Returns a flag that represents if the gripper is grasping something.
WARNING: THIS FUNCTION HAS BEEN DEPRECATED WHEN WE SWITCHED
TO THE FRANKA_ROS GRIPPER CONTROLLER BECAUSE IT DOES NOT PUBLISH
THE IS GRASPED FLAG. YOU WILL NEED TO DETERMINE WHEN THE ROBOT
IS GRASPING AN ITEM YOURSELF USING THE GRIPPER WIDTH.
Returns
-------
is_grasped : :obj:`bool`
Returns True if gripper is grasping something. False otherwise.
"""
return self._state_client.get_gripper_is_grasped()
def get_tool_base_pose(self):
"""
Returns the tool delta pose.
Returns
-------
tool_delta_pose : :obj:`autolab_core.RigidTransform`
RigidTransform that represents the transform between the
end-effector and the end of the tool.
"""
return self._tool_delta_pose.copy()
def get_ee_force_torque(self):
"""
Returns the current end-effector's sensed force_torque.
Returns
-------
ee_force_torque : :obj:`numpy.ndarray`
A numpy ndarray of 6 floats that represents the current
end-effector's sensed force_torque.
"""
return self._state_client.get_ee_force_torque()
def get_finger_poses(self):
"""
Returns the current poses of the left and right fingers.
Returns
-------
left_finger_pose : :obj:`autolab_core.RigidTransform`
RigidTransform that represents the transform of the left finger.
The left finger is +y from the gripper pose
right_finger_pose : :obj:`autolab_core.RigidTransform`
RigidTransform that represents the transform of the right finger.
The right finger is -y from the gripper pose
"""
ee_pose = self.get_pose()
delta_gripper_width = self.get_gripper_width() / 2
left_finger_pose = ee_pose * RigidTransform(
translation=np.array([0, delta_gripper_width, 0]),
from_frame='finger_left', to_frame=ee_pose.from_frame
)
right_finger_pose = ee_pose * RigidTransform(
translation=np.array([0, -delta_gripper_width, 0]),
from_frame='finger_right', to_frame=ee_pose.from_frame
)
return left_finger_pose, right_finger_pose
"""
Sets
"""
def set_tool_delta_pose(self, tool_delta_pose):
"""
Sets the tool pose relative to the end-effector pose
Parameters
----------
tool_delta_pose : :obj:`autolab_core.RigidTransform`
RigidTransform that represents the transform between the
end-effector and the end of the tool.
"""
if tool_delta_pose.from_frame != 'franka_tool' \
or tool_delta_pose.to_frame != 'franka_tool_base':
raise ValueError('tool_delta_pose has invalid frame names! ' \
'Make sure it has from_frame=franka_tool, and ' \
'to_frame=franka_tool_base')
self._tool_delta_pose = tool_delta_pose.copy()
"""
Forward Kinematics, Jacobian, other offline methods
"""
_dh_alpha_rot = np.array([
[1, 0, 0, 0],
[0, -1, -1, 0],
[0, -1, -1, 0],
[0, 0, 0, 1]
], dtype=np.float32)
_dh_a_trans = np.array([
[1, 0, 0, -1],
[0, 1, 0, 0],
[0, 0, 1, 0],
[0, 0, 0, 1]
], dtype=np.float32)
_dh_d_trans = np.array([
[1, 0, 0, 0],
[0, 1, 0, 0],
[0, 0, 1, -1],
[0, 0, 0, 1]
], dtype=np.float32)
_dh_theta_rot = np.array([
[-1, -1, 0, 0],
[-1, -1, 0, 0],
[0, 0, 1, 0],
[0, 0, 0, 1]
], dtype=np.float32)
def get_links_transforms(self, joints, use_rigid_transforms=False):
"""
Computes the forward kinematics of all links and the end-effector
Parameters
----------
joints : :obj:`list`
A list of 7 numbers that correspond to to the joint angles in radians
use_rigid_transforms : :obj:`bool`
Optional: Defaults to False.
If True, converts result to RigidTransform objects. This is slower.
Returns
-------
transforms : :obj:`list`
A list of 9 RigidTransforms or ndarrays in panda_link0 to panda_link7,
the franka_tool_base, and franka_tool frames.
"""
transforms_matrices = np.repeat(np.expand_dims(np.eye(4), 0), len(FC.DH_PARAMS) + 3, axis=0)
prev_transform = np.eye(4)
for i in range(len(FC.DH_PARAMS)):
a, d, alpha, theta = FC.DH_PARAMS[i]
if i < FC.N_REV_JOINTS:
theta = theta + joints[i]
ca, sa = np.cos(alpha), np.sin(alpha)
ct, st = np.cos(theta), np.sin(theta)
self._dh_alpha_rot[1, 1] = ca
self._dh_alpha_rot[1, 2] = -sa
self._dh_alpha_rot[2, 1] = sa
self._dh_alpha_rot[2, 2] = ca
self._dh_a_trans[0, 3] = a
self._dh_d_trans[2, 3] = d
self._dh_theta_rot[0, 0] = ct
self._dh_theta_rot[0, 1] = -st
self._dh_theta_rot[1, 0] = st
self._dh_theta_rot[1, 1] = ct
delta_transform_matrix = self._dh_alpha_rot @ self._dh_a_trans @ self._dh_d_trans @ self._dh_theta_rot
transforms_matrices[i + 1] = prev_transform @ delta_transform_matrix
prev_transform = transforms_matrices[i + 1]
transforms_matrices[10] = transforms_matrices[9]
transforms_matrices[11] = transforms_matrices[10] @ self._tool_delta_pose.matrix
rigid_transforms = []
if use_rigid_transforms:
for i in range(8):
rigid_transforms.append(
RigidTransform(rotation=transforms_matrices[i, :3, :3], translation=transforms_matrices[i, :3, 3],
from_frame='panda_link{}'.format(i + 1), to_frame='world'
))
rigid_transforms.append(
RigidTransform(rotation=transforms_matrices[8, :3, :3], translation=transforms_matrices[8, :3, 3],
from_frame='panda_hand', to_frame='world'
))
transform_tool_base = rigid_transforms[-1].as_frames(from_frame='franka_tool_base')
transform_tool = transform_tool_base * self._tool_delta_pose
rigid_transforms.append(transform_tool_base)
rigid_transforms.append(transform_tool)
return rigid_transforms
else:
return transforms_matrices
def get_jacobian(self, joints):
"""
Computes the geometric jacobian
Parameters
----------
joints : :obj:`list`
A list of 7 numbers that correspond to the joint angles in radians.
Returns
-------
jacobian : :obj:`numpy.ndarray`
A 6 by 7 jacobian matrix.
"""
transforms = self.get_links_transforms(joints, use_rigid_transforms=False)
joints_pos = transforms[1:FC.N_REV_JOINTS + 1, :3, 3]
ee_pos = transforms[-1, :3, 3]
axes = transforms[1:FC.N_REV_JOINTS + 1, :3, 2]
J = np.r_[np.cross(axes, ee_pos - joints_pos).T, axes.T]
return J
def get_collision_boxes_poses(self, joints=None, use_rigid_transforms=False):
"""
Computes the transforms of all collision boxes in world frame.
Parameters
----------
joints : :obj:`list`
Optional: Defaults to None
A list of 7 numbers that correspond to to the joint angles in radians
If None, will use current real robot joints.
use_rigid_transforms : :obj:`bool`
Optional: Defaults to False.
If True, converts result to RigidTransform objects. This is slower.
Returns
-------
transforms : :obj:`list`
A list of RigidTransforms or ndarrays for all collision boxes in world frame.
"""
if joints is None:
joints = self.get_joints()
fk = self.get_links_transforms(joints, use_rigid_transforms=False)
box_poses = []
for box_n, link in enumerate(FC.COLLISION_BOX_LINKS):
link_transform = fk[link]
box_pose_world = link_transform @ FC.COLLISION_BOX_POSES[box_n]
box_poses.append(box_pose_world)
if use_rigid_transforms:
box_transforms = [RigidTransform(
rotation=box_pose[:3, :3],
translation=box_pose[:3, 3],
from_frame='box{}'.format(box_n), to_frame='world'
) for box_n, box_pose in enumerate(box_poses)]
return box_transforms
else:
return box_poses
def publish_sensor_values(self, sensor_values=None):
"""
Publish sensor values to franka-interface
Parameters
----------
sensor_values : :obj:`SensorDataGroup`
A SensorDataGroup message.
"""
if sensor_values is None:
return False
self._sensor_pub.publish(sensor_values)
def publish_joints(self, joints=None):
"""
Publish the Franka joints to ROS
Parameters
----------
joints : :obj:`list`
Optional: Defaults to None
A list of 7 numbers that correspond to to the joint angles in radians
If None, will use current real robot joints.
"""
if joints is None:
joints = self.get_joints()
joint_state = JointState()
joint_state.name = FC.JOINT_NAMES
joint_state.header.stamp = rospy.Time.now()
if len(joints) == 7:
joints = np.concatenate([joints, [0, 0]])
joint_state.position = joints
self._joint_state_pub.publish(joint_state)
def publish_collision_boxes(self, joints=None):
"""
Publish the Franka collsion boxes to ROS
Parameters
----------
joints : :obj:`list`
Optional: Defaults to None
A list of 7 numbers that correspond to to the joint angles in radians.
If None, will use current real robot joints.
"""
if joints is None:
joints = self.get_joints()
box_poses_world = self.get_collision_boxes_poses(joints)
for i, pose in enumerate(box_poses_world):
self._collision_boxes_data[i, :3] = pose[:3, 3]
q = quaternion.from_rotation_matrix(pose[:3, :3])
for j, k in enumerate('wxyz'):
self._collision_boxes_data[i, 3 + j] = getattr(q, k)
self._collision_boxes_pub.publish_boxes(self._collision_boxes_data)
def check_box_collision(self, box, joints=None):
"""
Checks if the given joint configuration is in collision with a box
Parameters
----------
joints : :obj:`list`
Optional: Defaults to None
A list of 7 numbers that correspond to to the joint angles in radians
If None, will use current real robot joints.
box : :obj:`list`
The position of the center of the box [x, y, z, r, p, y] and the
length, width, and height [l, w, h].
Returns
-------
in_collision : :obj:`bool`
Flag that indicates whether the robot would be in collision.
"""
box_pos, box_rpy, box_hsizes = box[:3], box[3:6], box[6:]/2
box_q = quaternion.from_euler_angles(box_rpy)
box_axes = quaternion.as_rotation_matrix(box_q)
self._box_vertices_offset[:,:] = self._vertex_offset_signs * box_hsizes
box_vertices = (box_axes @ self._box_vertices_offset.T + np.expand_dims(box_pos, 1)).T
box_hdiag = np.linalg.norm(box_hsizes)
min_col_dists = box_hdiag + self._collision_box_hdiags
franka_box_poses = self.get_collision_boxes_poses(joints)
for i, franka_box_pose in enumerate(franka_box_poses):
fbox_pos = franka_box_pose[:3, 3]
fbox_axes = franka_box_pose[:3, :3]
# coarse collision check
if np.linalg.norm(fbox_pos - box_pos) > min_col_dists[i]:
continue
fbox_vertex_offsets = self._collision_box_vertices_offset[i]
fbox_vertices = fbox_vertex_offsets @ fbox_axes.T + fbox_pos
# construct axes
cross_product_pairs = np.array(list(product(box_axes.T, fbox_axes.T)))
cross_axes = np.cross(cross_product_pairs[:,0], cross_product_pairs[:,1]).T
self._collision_proj_axes[:, :3] = box_axes
self._collision_proj_axes[:, 3:6] = fbox_axes
self._collision_proj_axes[:, 6:] = cross_axes
# projection
box_projs = box_vertices @ self._collision_proj_axes
fbox_projs = fbox_vertices @ self._collision_proj_axes
min_box_projs, max_box_projs = box_projs.min(axis=0), box_projs.max(axis=0)
min_fbox_projs, max_fbox_projs = fbox_projs.min(axis=0), fbox_projs.max(axis=0)
# check if no separating planes exist
if np.all([min_box_projs <= max_fbox_projs, max_box_projs >= min_fbox_projs]):
return True
return False
def is_joints_in_collision_with_boxes(self, joints=None, boxes=None):
"""
Checks if the given joint configuration is in collision with boxes
or the workspace walls.
Parameters
----------
joints : :obj:`list`
Optional: Defaults to None
A list of 7 numbers that correspond to the joint angles in radians
If None, will use current real robot joints.
boxes : :obj:`list`
List of boxes where each box is a list of 9 floats that contains
the position of the center of the box [x, y, z, r, p, y] and the
length, width, and height [l, w, h].
Returns
-------
in_collision : :obj:`bool`
Flag that indicates whether the robot would be in collision.
"""
if boxes is None:
boxes = FC.WORKSPACE_WALLS
for box in boxes:
if self.check_box_collision(box, joints=joints):
return True
return False
"""
Misc
"""
def reset_joints(self, duration=5, block=True, ignore_errors=True, skill_desc=''):
"""
Commands Arm to go to hardcoded home joint configuration
Parameters
----------
duration : :obj:`float`
How much time this robot motion should take.
block : :obj:`bool`
Function blocks by default. If False, the function becomes
asynchronous and can be preempted.
ignore_errors : :obj:`bool`
Function ignores errors by default. If False, errors and some
exceptions can be thrown.
skill_desc : :obj:`str`
Skill description to use for logging on control-pc.
"""
self.goto_joints(FC.HOME_JOINTS, duration=duration, skill_desc=skill_desc, block=block, ignore_errors=ignore_errors)
def reset_pose(self, duration=5, block=True, ignore_errors=True, skill_desc=''):
"""
Commands Arm to go to hardcoded home pose
Parameters
----------
duration : :obj:`float`
How much time this robot motion should take.
block : :obj:`bool`
Function blocks by default. If False, the function becomes
asynchronous and can be preempted.
ignore_errors : :obj:`bool`
Function ignores errors by default. If False, errors and some
exceptions can be thrown.
skill_desc : :obj:`str`
Skill description to use for logging on control-pc.
"""
self.goto_pose(FC.HOME_POSE, duration=duration, skill_desc=skill_desc, block=block, ignore_errors=ignore_errors)
def is_joints_reachable(self, joints):
"""
Checks if the given joint configuration is within joint limits.
Parameters
----------
joints : :obj:`list`
A list of 7 numbers that correspond to the joint angles in radians.
Returns
-------
joints_reachable : :obj:`bool`
Flag that is True if all joints are within joint limits.
"""
for i, val in enumerate(joints):
if val <= FC.JOINT_LIMITS_MIN[i] or val >= FC.JOINT_LIMITS_MAX[i]:
return False
return True
"""
Unimplemented
"""
def apply_joint_torques(self, torques, duration, ignore_errors=True, skill_desc='',):
"""
NOT IMPLEMENTED YET
Commands the arm to apply given joint torques for duration seconds.
Parameters
----------
torques : :obj:`list`
A list of 7 numbers that correspond to torques in Nm.
duration : :obj:`float`
How much time this robot motion should take.
skill_desc : :obj:`str`
Skill description to use for logging on control-pc.
"""
pass
def set_speed(self, speed):
"""
NOT IMPLEMENTED YET
Sets the current target speed parameter
Parameters
----------
speed : :obj:`float`
Current target speed parameter
"""
pass
def get_speed(self, speed):
"""
NOT IMPLEMENTED YET
Returns the current target speed parameter
Returns
-------
speed : :obj:`float`
Current target speed parameter
"""
pass
================================================
FILE: frankapy/franka_arm_state_client.py
================================================
import logging
import numpy as np
import rospy
from franka_interface_msgs.srv import GetCurrentRobotStateCmd
from .utils import franka_pose_to_rigid_transform
class FrankaArmStateClient:
def __init__(self, new_ros_node=True, robot_state_server_name='/get_current_robot_state_server_node_1/get_current_robot_state_server', offline=False):
if new_ros_node:
rospy.init_node('FrankaArmStateClient', anonymous=True)
self._offline = offline
if not self._offline:
rospy.wait_for_service(robot_state_server_name)
self._get_current_robot_state = rospy.ServiceProxy(robot_state_server_name, GetCurrentRobotStateCmd)
def get_data(self):
'''Get all fields of current robot data in a dict.
Returns:
dict of robot state
'''
if self._offline:
logging.warn('In offline mode - FrankaArmStateClient will return 0 values.')
return {
'cartesian_velocities': np.zeros(6),
'pose': franka_pose_to_rigid_transform(np.eye(4)),
'pose_desired': franka_pose_to_rigid_transform(np.eye(4)),
'joint_torques': np.zeros(7),
'joint_torques_derivative': np.zeros(7),
'joints': np.zeros(7),
'joints_desired': np.zeros(7),
'joint_velocities': np.zeros(7),
'gripper_width': 0,
'gripper_is_grasped': False,
'ee_force_torque': np.zeros(6)
}
ros_data = self._get_current_robot_state().robot_state
data = {
'cartesian_velocities': np.array(ros_data.O_dP_EE_c),
'pose': franka_pose_to_rigid_transform(ros_data.O_T_EE),
'pose_desired': franka_pose_to_rigid_transform(ros_data.O_T_EE_d),
'joint_torques': np.array(ros_data.tau_J),
'joint_torques_derivative': np.array(ros_data.dtau_J),
'joints': np.array(ros_data.q),
'joints_desired': np.array(ros_data.q_d),
'joint_velocities': np.array(ros_data.dq),
'gripper_width': ros_data.gripper_width,
'gripper_is_grasped': ros_data.gripper_is_grasped,
'ee_force_torque': np.array(ros_data.O_F_ext_hat_K)
}
return data
def get_pose(self):
'''Get the current pose.
Returns:
RigidTransform
'''
return self.get_data()['pose']
def get_joints(self):
'''Get the current joint configuration.
Returns:
ndarray of shape (7,)
'''
return self.get_data()['joints']
def get_joint_torques(self):
'''Get the current joint torques.
Returns:
ndarray of shape (7,)
'''
return self.get_data()['joint_torques']
def get_joint_velocities(self):
'''Get the current joint velocities.
Returns:
ndarray of shape (7,)
'''
return self.get_data()['joint_velocities']
def get_gripper_width(self):
'''Get most recent gripper width. Note this value will *not* be updated
during a control command.
Returns:
float of gripper width in meters
'''
return self.get_data()['gripper_width']
def get_gripper_is_grasped(self):
'''Returns whether or not the gripper is grasping something. Note this
value will *not* be updated during a control command.
Returns:
True if gripper is grasping something. False otherwise
'''
return self.get_data()['gripper_is_grasped']
def get_ee_force_torque(self):
'''Get the current ee force torque in base frame.
Returns:
ndarray of shape (6,)
'''
return self.get_data()['ee_force_torque']
================================================
FILE: frankapy/franka_constants.py
================================================
import logging
import math
import numpy as np
from autolab_core import RigidTransform
class FrankaConstants:
'''
Contains default robot values, as well as robot limits.
All units are in SI.
'''
LOGGING_LEVEL = logging.INFO
EMPTY_SENSOR_VALUES = [0]
# translational stiffness, rotational stiffness
DEFAULT_FORCE_AXIS_TRANSLATIONAL_STIFFNESS = 600
DEFAULT_FORCE_AXIS_ROTATIONAL_STIFFNESS = 20
# buffer time
DEFAULT_TERM_BUFFER_TIME = 0.2
HOME_JOINTS = [0, -math.pi / 4, 0, -3 * math.pi / 4, 0, math.pi / 2, math.pi / 4]
HOME_POSE = RigidTransform(rotation=np.array([
[1, 0, 0],
[0, -1, 0],
[0, 0, -1],
]), translation=np.array([0.3069, 0, 0.4867]),
from_frame='franka_tool', to_frame='world')
READY_JOINTS = [0, -math.pi/4, 0, -2.85496998, 0, 2.09382820, math.pi/4]
READY_POSE = RigidTransform(rotation=np.array([
[1, 0, 0],
[0, -1, 0],
[0, 0, -1],
]), translation=np.array([0.3069, 0, 0.2867]),
from_frame='franka_tool', to_frame='world')
# See https://frankaemika.github.io/docs/control_parameters.html
JOINT_LIMITS_MIN = [-2.8973, -1.7628, -2.8973, -3.0718, -2.8973, -0.0175, -2.8973]
JOINT_LIMITS_MAX = [2.8973, 1.7628, 2.8973, -0.0698, 2.8973, 3.7525, 2.8973]
DEFAULT_POSE_THRESHOLDS = [0.0001, 0.0001, 0.0001, 0.0001, 0.0001, 0.0001]
DEFAULT_JOINT_THRESHOLDS = [0.0001, 0.0001, 0.0001, 0.0001, 0.0001, 0.0001, 0.0001]
GRIPPER_WIDTH_MAX = 0.08
GRIPPER_WIDTH_MIN = 0
GRIPPER_MAX_FORCE = 60
MAX_LIN_MOMENTUM = 20
MAX_ANG_MOMENTUM = 2
MAX_LIN_MOMENTUM_CONSTRAINED = 100
DEFAULT_FRANKA_INTERFACE_TIMEOUT = 10
ACTION_WAIT_LOOP_TIME = 0.001
GRIPPER_CMD_SLEEP_TIME = 0.2
DEFAULT_K_GAINS = [600.0, 600.0, 600.0, 600.0, 250.0, 150.0, 50.0]
DEFAULT_D_GAINS = [50.0, 50.0, 50.0, 50.0, 30.0, 25.0, 15.0]
DEFAULT_TRANSLATIONAL_STIFFNESSES = [600.0, 600.0, 600.0]
DEFAULT_ROTATIONAL_STIFFNESSES = [50.0, 50.0, 50.0]
DEFAULT_JOINT_IMPEDANCES = [3000, 3000, 3000, 2500, 2500, 2000, 2000]
DEFAULT_CARTESIAN_IMPEDANCES = [3000, 3000, 3000, 300, 300, 300]
DEFAULT_LOWER_TORQUE_THRESHOLDS_ACCEL = [20.0,20.0,18.0,18.0,16.0,14.0,12.0]
DEFAULT_UPPER_TORQUE_THRESHOLDS_ACCEL = [120.0,120.0,120.0,118.0,116.0,114.0,112.0]
DEFAULT_LOWER_TORQUE_THRESHOLDS_NOMINAL = [20.0,20.0,18.0,18.0,16.0,14.0,12.0]
DEFAULT_UPPER_TORQUE_THRESHOLDS_NOMINAL = [120.0,120.0,118.0,118.0,116.0,114.0,112.0]
DEFAULT_LOWER_FORCE_THRESHOLDS_ACCEL = [10.0,10.0,10.0,10.0,10.0,10.0]
DEFAULT_UPPER_FORCE_THRESHOLDS_ACCEL = [120.0,120.0,120.0,125.0,125.0,125.0]
DEFAULT_LOWER_FORCE_THRESHOLDS_NOMINAL = [10.0,10.0,10.0,10.0,10.0,10.0]
DEFAULT_UPPER_FORCE_THRESHOLDS_NOMINAL = [120.0,120.0,120.0,125.0,125.0,125.0]
DH_PARAMS = np.array([[0, 0.333, 0, 0],
[0, 0, -np.pi/2, 0],
[0, 0.316, np.pi/2, 0],
[0.0825, 0, np.pi/2, 0],
[-0.0825, 0.384, -np.pi/2, 0],
[0, 0, np.pi/2, 0],
[0.088, 0, np.pi/2, 0],
[0, 0.107, 0, 0],
[0, 0.1034, 0, 0]])
N_REV_JOINTS = 7
COLLISION_BOX_SHAPES = np.array([
[0.23, 0.2, 0.1],
[0.13, 0.12, 0.1],
[0.12, 0.1, 0.2],
[0.15, 0.27, 0.11],
[0.12, 0.1, 0.2],
[0.13, 0.12, 0.25],
[0.13, 0.23, 0.15],
[0.12, 0.12, 0.4],
[0.12, 0.12, 0.25],
[0.13, 0.23, 0.12],
[0.12, 0.12, 0.2],
[0.08, 0.22, 0.17]
])
COLLISION_BOX_LINKS = [1, 1, 1, 1, 1, 3, 4, 5, 5, 5, 7, 7]
COLLISION_BOX_POSES = np.array([
[[ 1. , 0. , 0. , -0.04 ],
[ 0. , 1. , 0. , 0. ],
[ 0. , 0. , 1. , -0.283 ],
[ 0. , 0. , 0. , 1. ]],
[[ 1. , 0. , 0. , -0.009 ],
[ 0. , 1. , 0. , 0. ],
[ 0. , 0. , 1. , -0.183 ],
[ 0. , 0. , 0. , 1. ]],
[[ 1. , 0. , 0. , 0. ],
[ 0. , 0.81038486, -0.58589793, -0.032 ],
[ 0. , 0.58589793, 0.81038486, -0.082 ],
[ 0. , 0. , 0. , 1. ]],
[[ 1. , 0. , 0. , -0.008 ],
[ 0. , 1. , 0. , 0. ],
[ 0. , 0. , 1. , 0. ],
[ 0. , 0. , 0. , 1. ]],
[[ 1. , 0. , 0. , 0. ],
[ 0. , 0.81038486, -0.58589793, 0.042 ],
[ 0. , 0.58589793, 0.81038486, 0.067 ],
[ 0. , 0. , 0. , 1. ]],
[[ 1. , 0. , 0. , 0.00687 ],
[ 0. , 1. , 0. , 0. ],
[ 0. , 0. , 1. , -0.139 ],
[ 0. , 0. , 0. , 1. ]],
[[ 1. , -0. , 0. , -0.008 ],
[ 0. , 0. , 1. , 0.004 ],
[-0. , -1. , 0. , 0. ],
[ 0. , 0. , 0. , 1. ]],
[[ 1. , -0. , 0. , 0.00422 ],
[ 0. , 0.98480775, 0.17364817, 0.05367 ],
[-0. , -0.17364817, 0.98480775, -0.121 ],
[ 0. , 0. , 0. , 1. ]],
[[ 1. , 0. , 0. , 0.00422 ],
[ 0. , 1. , 0. , 0.00367 ],
[ 0. , 0. , 1. , -0.263 ],
[ 0. , 0. , 0. , 1. ]],
[[ 1. , 0. , 0. , 0.00328 ],
[ 0. , 1. , 0. , 0.0176 ],
[ 0. , 0. , 1. , -0.0055 ],
[ 0. , 0. , 0. , 1. ]],
[[ 1. , 0. , 0. , -0.0136 ],
[ 0. , -1. , 0. , 0.0092 ],
[ 0. , 0. , -1. , 0.0083 ],
[ 0. , 0. , 0. , 1. ]],
[[ 0.70710678, 0.70710678, 0. , 0.0136 ],
[-0.70710678, 0.70710678, -0. , -0.0092 ],
[-0. , 0. , 1. , 0.1457 ],
[ 0. , 0. , 0. , 1. ]]]
)
JOINT_NAMES = ['panda_joint1',
'panda_joint2',
'panda_joint3',
'panda_joint4',
'panda_joint5',
'panda_joint6',
'panda_joint7',
'panda_finger_joint1',
'panda_finger_joint2']
WORKSPACE_WALLS = np.array([
# sides
[0.15, 0.46, 0.5, 0, 0, 0, 1.2, 0.01, 1.1],
[0.15, -0.46, 0.5, 0, 0, 0, 1.2, 0.01, 1.1],
# back
[-0.41, 0, 0.5, 0, 0, 0, 0.01, 1, 1.1],
# front
[0.75, 0, 0.5, 0, 0, 0, 0.01, 1, 1.1],
# top
[0.2, 0, 1, 0, 0, 0, 1.2, 1, 0.01],
# bottom
[0.2, 0, -0.05, 0, 0, 0, 1.2, 1, 0.01]
])
DEFAULT_SENSOR_PUBLISHER_TOPIC = 'franka_ros_interface/sensor'
DYNAMIC_SKILL_WAIT_TIME = 0.3
DEFAULT_HFPC_FORCE_GAIN = [0.1] * 6
DEFAULT_HFPC_S = [1, 1, 1, 1, 1, 1]
================================================
FILE: frankapy/franka_interface_common_definitions.py
================================================
#######################################################################
# #
# Important: Any Changes here should also be reflected in changes #
# in the franka-interface-common definitions.h file as well. #
# #
# The order of the enums matter!! #
# #
#######################################################################
_ENUM_COUNTER = {}
def _enum_auto(key):
if key not in _ENUM_COUNTER:
_ENUM_COUNTER[key] = 0
val = _ENUM_COUNTER[key]
_ENUM_COUNTER[key] += 1
return val
class SkillType:
CartesianPoseSkill = _enum_auto('SkillType')
CartesianVelocitySkill = _enum_auto('SkillType')
ForceTorqueSkill = _enum_auto('SkillType')
GripperSkill = _enum_auto('SkillType')
ImpedanceControlSkill = _enum_auto('SkillType')
JointPositionSkill = _enum_auto('SkillType')
JointVelocitySkill = _enum_auto('SkillType')
class MetaSkillType:
BaseMetaSkill = _enum_auto('MetaSkillType')
JointPositionContinuousSkill = _enum_auto('MetaSkillType')
class TrajectoryGeneratorType:
CartesianVelocityTrajectoryGenerator = _enum_auto('TrajectoryGeneratorType')
CubicHermiteSplineJointTrajectoryGenerator = _enum_auto('TrajectoryGeneratorType')
CubicHermiteSplinePoseTrajectoryGenerator = _enum_auto('TrajectoryGeneratorType')
GoalPoseDmpTrajectoryGenerator = _enum_auto('TrajectoryGeneratorType')
GripperTrajectoryGenerator = _enum_auto('TrajectoryGeneratorType')
ImpulseTrajectoryGenerator = _enum_auto('TrajectoryGeneratorType')
JointDmpTrajectoryGenerator = _enum_auto('TrajectoryGeneratorType')
JointVelocityTrajectoryGenerator = _enum_auto('TrajectoryGeneratorType')
LinearForcePositionTrajectoryGenerator = _enum_auto('TrajectoryGeneratorType')
LinearJointTrajectoryGenerator = _enum_auto('TrajectoryGeneratorType')
LinearPoseTrajectoryGenerator = _enum_auto('TrajectoryGeneratorType')
MinJerkJointTrajectoryGenerator = _enum_auto('TrajectoryGeneratorType')
MinJerkPoseTrajectoryGenerator = _enum_auto('TrajectoryGeneratorType')
PassThroughCartesianVelocityTrajectoryGenerator = _enum_auto('TrajectoryGeneratorType')
PassThroughForcePositionTrajectoryGenerator = _enum_auto('TrajectoryGeneratorType')
PassThroughJointTrajectoryGenerator = _enum_auto('TrajectoryGeneratorType')
PassThroughJointVelocityTrajectoryGenerator = _enum_auto('TrajectoryGeneratorType')
PassThroughPoseTrajectoryGenerator = _enum_auto('TrajectoryGeneratorType')
PoseDmpTrajectoryGenerator = _enum_auto('TrajectoryGeneratorType')
QuaternionPoseDmpTrajectoryGenerator = _enum_auto('TrajectoryGeneratorType')
RelativeLinearPoseTrajectoryGenerator = _enum_auto('TrajectoryGeneratorType')
RelativeMinJerkPoseTrajectoryGenerator = _enum_auto('TrajectoryGeneratorType')
SineJointTrajectoryGenerator = _enum_auto('TrajectoryGeneratorType')
SinePoseTrajectoryGenerator = _enum_auto('TrajectoryGeneratorType')
StayInInitialJointsTrajectoryGenerator = _enum_auto('TrajectoryGeneratorType')
StayInInitialPoseTrajectoryGenerator = _enum_auto('TrajectoryGeneratorType')
class FeedbackControllerType:
CartesianImpedanceFeedbackController = _enum_auto('FeedbackControllerType')
EECartesianImpedanceFeedbackController = _enum_auto('FeedbackControllerType')
ForceAxisImpedenceFeedbackController = _enum_auto('FeedbackControllerType')
ForcePositionFeedbackController = _enum_auto('FeedbackControllerType')
JointImpedanceFeedbackController = _enum_auto('FeedbackControllerType')
NoopFeedbackController = _enum_auto('FeedbackControllerType')
PassThroughFeedbackController = _enum_auto('FeedbackControllerType')
PassThroughJointTorqueFeedbackController = _enum_auto('FeedbackControllerType')
SetInternalImpedanceFeedbackController = _enum_auto('FeedbackControllerType')
class TerminationHandlerType:
ContactTerminationHandler = _enum_auto('TerminationHandlerType')
FinalJointTerminationHandler = _enum_auto('TerminationHandlerType')
FinalPoseTerminationHandler = _enum_auto('TerminationHandlerType')
NoopTerminationHandler = _enum_auto('TerminationHandlerType')
gitextract_4vicwimb/
├── .github/
│ └── workflows/
│ └── docs_pages_workflow.yml
├── .gitignore
├── .gitmodules
├── LICENSE
├── README.md
├── bash_scripts/
│ ├── make_catkin.sh
│ ├── make_proto.sh
│ ├── start_control_pc.sh
│ ├── start_franka_gripper_on_control_pc.sh
│ ├── start_franka_interface_on_control_pc.sh
│ ├── start_franka_ros_interface_on_control_pc.sh
│ └── start_rosmaster.sh
├── cfg/
│ └── april_tag_pick_place_cfg.yaml
├── docs/
│ ├── .gitignore
│ ├── Makefile
│ ├── buildDocs.sh
│ ├── conf.py
│ ├── frankaarm.rst
│ ├── index.rst
│ ├── install.rst
│ ├── make.bat
│ ├── network.rst
│ ├── running.rst
│ └── support.rst
├── examples/
│ ├── T_RSD415_franka.tf
│ ├── T_RS_mount_delta.tf
│ ├── april_tag_pick_place.py
│ ├── example_movements.py
│ ├── execute_joint_torques.py
│ ├── go_to_joints_with_joint_impedances.py
│ ├── move_robot.py
│ ├── record_trajectory.py
│ ├── run_dynamic_cartesian_velocities.py
│ ├── run_dynamic_joint_velocities.py
│ ├── run_dynamic_joints.py
│ ├── run_dynamic_pose.py
│ ├── run_hfpc.py
│ ├── run_pose_dmp.py
│ ├── run_quaternion_pose_dmp.py
│ └── run_recorded_trajectory.py
├── frankapy/
│ ├── __init__.py
│ ├── exceptions.py
│ ├── franka_arm.py
│ ├── franka_arm_state_client.py
│ ├── franka_constants.py
│ ├── franka_interface_common_definitions.py
│ ├── proto/
│ │ ├── __init__.py
│ │ ├── feedback_controller_params_msg.proto
│ │ ├── feedback_controller_params_msg_pb2.py
│ │ ├── sensor_msg.proto
│ │ ├── sensor_msg_pb2.py
│ │ ├── termination_handler_params_msg.proto
│ │ ├── termination_handler_params_msg_pb2.py
│ │ ├── trajectory_generator_params_msg.proto
│ │ └── trajectory_generator_params_msg_pb2.py
│ ├── proto_utils.py
│ ├── ros_utils.py
│ ├── skill_list.py
│ └── utils.py
├── launch/
│ └── franka_rviz.launch
├── rviz/
│ └── franka_basic_config.rviz
├── scripts/
│ ├── close_gripper.py
│ ├── open_gripper.py
│ ├── record_trajectory.py
│ ├── reset_arm.py
│ ├── run_guide_mode.py
│ ├── run_guide_mode_ee_frame.py
│ ├── run_recorded_trajectory.py
│ └── test_virtual_walls.py
├── setup.py
└── urdf/
├── hand.urdf.xacro
├── hand.xacro
├── panda_arm.urdf.xacro
├── panda_arm.xacro
└── panda_arm_hand.urdf.xacro
SYMBOL INDEX (134 symbols across 13 files)
FILE: examples/april_tag_pick_place.py
function subsample (line 17) | def subsample(data, rate=0.1):
function make_det_one (line 22) | def make_det_one(R):
function get_closest_grasp_pose (line 27) | def get_closest_grasp_pose(T_tag_world, T_ee_world):
FILE: examples/go_to_joints_with_joint_impedances.py
function wait_for_enter (line 6) | def wait_for_enter():
FILE: examples/run_quaternion_pose_dmp.py
function execute_quaternion_pose_dmp (line 9) | def execute_quaternion_pose_dmp(fa, position_dmp_weights_path, quat_dmp_...
FILE: frankapy/exceptions.py
class FrankaArmCommException (line 1) | class FrankaArmCommException(Exception):
method __init__ (line 4) | def __init__(self, message, *args, **kwargs):
method __str__ (line 8) | def __str__(self):
class FrankaArmFrankaInterfaceNotReadyException (line 12) | class FrankaArmFrankaInterfaceNotReadyException(Exception):
method __init__ (line 16) | def __init__(self, *args, **kwargs):
method __str__ (line 19) | def __str__(self):
class FrankaArmException (line 23) | class FrankaArmException(Exception):
method __init__ (line 27) | def __init__(self, message, *args, **kwargs):
method __str__ (line 31) | def __str__(self):
FILE: frankapy/franka_arm.py
class FrankaArm (line 31) | class FrankaArm:
method __init__ (line 33) | def __init__(
method wait_for_franka_interface (line 165) | def wait_for_franka_interface(self, timeout=None):
method wait_for_skill (line 184) | def wait_for_skill(self):
method wait_for_gripper (line 191) | def wait_for_gripper(self):
method is_skill_done (line 206) | def is_skill_done(self, ignore_errors=True):
method stop_skill (line 247) | def stop_skill(self):
method _sigint_handler_gen (line 255) | def _sigint_handler_gen(self):
method _send_goal (line 263) | def _send_goal(self, goal, cb, block=True, ignore_errors=True):
method goto_pose (line 313) | def goto_pose(self,
method goto_pose_delta (line 432) | def goto_pose_delta(self,
method goto_joints (line 561) | def goto_joints(self,
method execute_cartesian_velocities (line 685) | def execute_cartesian_velocities(self,
method execute_joint_velocities (line 779) | def execute_joint_velocities(self,
method execute_joint_torques (line 875) | def execute_joint_torques(self,
method execute_joint_dmp (line 984) | def execute_joint_dmp(self,
method execute_pose_dmp (line 1082) | def execute_pose_dmp(self,
method execute_quaternion_pose_dmp (line 1203) | def execute_quaternion_pose_dmp(self,
method apply_effector_forces_torques (line 1306) | def apply_effector_forces_torques(self,
method apply_effector_forces_along_axis (line 1397) | def apply_effector_forces_along_axis(self,
method goto_gripper (line 1480) | def goto_gripper(self,
method selective_guidance_mode (line 1565) | def selective_guidance_mode(self,
method open_gripper (line 1689) | def open_gripper(self, block=True, skill_desc='OpenGripper'):
method close_gripper (line 1703) | def close_gripper(self, grasp=True, block=True, skill_desc='CloseGripp...
method home_gripper (line 1721) | def home_gripper(self, block=True, skill_desc='HomeGripper'):
method stop_gripper (line 1739) | def stop_gripper(self, block=True, skill_desc='StopGripper'):
method run_guide_mode (line 1757) | def run_guide_mode(self, duration=10, block=True, skill_desc='GuideMod...
method run_dynamic_force_position (line 1773) | def run_dynamic_force_position(self,
method get_robot_state (line 1865) | def get_robot_state(self):
method get_pose (line 1875) | def get_pose(self, include_tool_offset=True):
method get_joints (line 1891) | def get_joints(self):
method get_joint_torques (line 1902) | def get_joint_torques(self):
method get_joint_velocities (line 1913) | def get_joint_velocities(self):
method get_gripper_width (line 1924) | def get_gripper_width(self):
method get_gripper_is_grasped (line 1939) | def get_gripper_is_grasped(self):
method get_tool_base_pose (line 1955) | def get_tool_base_pose(self):
method get_ee_force_torque (line 1967) | def get_ee_force_torque(self):
method get_finger_poses (line 1979) | def get_finger_poses(self):
method set_tool_delta_pose (line 2009) | def set_tool_delta_pose(self, tool_delta_pose):
method get_links_transforms (line 2057) | def get_links_transforms(self, joints, use_rigid_transforms=False):
method get_jacobian (line 2131) | def get_jacobian(self, joints):
method get_collision_boxes_poses (line 2155) | def get_collision_boxes_poses(self, joints=None, use_rigid_transforms=...
method publish_sensor_values (line 2196) | def publish_sensor_values(self, sensor_values=None):
method publish_joints (line 2210) | def publish_joints(self, joints=None):
method publish_collision_boxes (line 2234) | def publish_collision_boxes(self, joints=None):
method check_box_collision (line 2259) | def check_box_collision(self, box, joints=None):
method is_joints_in_collision_with_boxes (line 2320) | def is_joints_in_collision_with_boxes(self, joints=None, boxes=None):
method reset_joints (line 2354) | def reset_joints(self, duration=5, block=True, ignore_errors=True, ski...
method reset_pose (line 2373) | def reset_pose(self, duration=5, block=True, ignore_errors=True, skill...
method is_joints_reachable (line 2392) | def is_joints_reachable(self, joints):
method apply_joint_torques (line 2416) | def apply_joint_torques(self, torques, duration, ignore_errors=True, s...
method set_speed (line 2433) | def set_speed(self, speed):
method get_speed (line 2446) | def get_speed(self, speed):
FILE: frankapy/franka_arm_state_client.py
class FrankaArmStateClient (line 10) | class FrankaArmStateClient:
method __init__ (line 12) | def __init__(self, new_ros_node=True, robot_state_server_name='/get_cu...
method get_data (line 21) | def get_data(self):
method get_pose (line 61) | def get_pose(self):
method get_joints (line 69) | def get_joints(self):
method get_joint_torques (line 77) | def get_joint_torques(self):
method get_joint_velocities (line 85) | def get_joint_velocities(self):
method get_gripper_width (line 93) | def get_gripper_width(self):
method get_gripper_is_grasped (line 102) | def get_gripper_is_grasped(self):
method get_ee_force_torque (line 111) | def get_ee_force_torque(self):
FILE: frankapy/franka_constants.py
class FrankaConstants (line 6) | class FrankaConstants:
FILE: frankapy/franka_interface_common_definitions.py
function _enum_auto (line 12) | def _enum_auto(key):
class SkillType (line 20) | class SkillType:
class MetaSkillType (line 30) | class MetaSkillType:
class TrajectoryGeneratorType (line 35) | class TrajectoryGeneratorType:
class FeedbackControllerType (line 64) | class FeedbackControllerType:
class TerminationHandlerType (line 76) | class TerminationHandlerType:
class SkillStatus (line 84) | class SkillStatus:
class SensorDataMessageType (line 91) | class SensorDataMessageType:
FILE: frankapy/proto_utils.py
function sensor_proto2ros_msg (line 4) | def sensor_proto2ros_msg(sensor_proto_msg, sensor_data_type, info=''):
function make_sensor_group_msg (line 16) | def make_sensor_group_msg(trajectory_generator_sensor_msg=None, feedback...
FILE: frankapy/ros_utils.py
class BoxesPublisher (line 6) | class BoxesPublisher:
method __init__ (line 8) | def __init__(self, name, world_frame='panda_link0'):
method publish_boxes (line 12) | def publish_boxes(self, boxes):
FILE: frankapy/skill_list.py
class Skill (line 15) | class Skill:
method __init__ (line 16) | def __init__(self,
method set_meta_skill_type (line 55) | def set_meta_skill_type(self, meta_skill_type):
method set_meta_skill_id (line 58) | def set_meta_skill_id(self, meta_skill_id):
method add_initial_sensor_values (line 61) | def add_initial_sensor_values(self, values):
method add_trajectory_params (line 65) | def add_trajectory_params(self, params):
method add_feedback_controller_params (line 69) | def add_feedback_controller_params(self, params):
method add_termination_params (line 73) | def add_termination_params(self, params):
method add_timer_params (line 77) | def add_timer_params(self, params):
method set_cartesian_impedances (line 83) | def set_cartesian_impedances(self, use_impedance, cartesian_impedances...
method set_joint_impedances (line 97) | def set_joint_impedances(self, use_impedance, cartesian_impedances, jo...
method add_cartesian_impedances (line 111) | def add_cartesian_impedances(self, cartesian_impedances):
method add_force_axis_params (line 126) | def add_force_axis_params(self, translational_stiffness, rotational_st...
method add_internal_impedances (line 143) | def add_internal_impedances(self, cartesian_impedances, joint_impedanc...
method add_joint_torques (line 161) | def add_joint_torques(self, joint_torques, selection, remove_gravity, ...
method add_force_position_params (line 180) | def add_force_position_params(self, position_kps_cart, force_kps_cart,...
method add_joint_gains (line 202) | def add_joint_gains(self, k_gains, d_gains):
method check_for_contact_params (line 217) | def check_for_contact_params(self, buffer_time, force_thresholds, torq...
method add_contact_termination_params (line 232) | def add_contact_termination_params(self, buffer_time,
method add_joint_threshold_params (line 255) | def add_joint_threshold_params(self, buffer_time, joint_thresholds):
method add_pose_threshold_params (line 271) | def add_pose_threshold_params(self, buffer_time, pose_thresholds):
method add_time_termination_params (line 288) | def add_time_termination_params(self, buffer_time):
method add_gripper_params (line 301) | def add_gripper_params(self, grasp, width, speed, force):
method add_impulse_params (line 323) | def add_impulse_params(self, run_time, acc_time, max_trans, max_rot, f...
method add_goal_cartesian_velocities (line 347) | def add_goal_cartesian_velocities(self, run_time, cartesian_velocities...
method add_goal_pose (line 361) | def add_goal_pose(self, run_time, goal_pose):
method add_goal_joints (line 373) | def add_goal_joints(self, run_time, joints):
method add_goal_joint_velocities (line 384) | def add_goal_joint_velocities(self, run_time, joint_velocities, joint_...
method add_joint_dmp_params (line 398) | def add_joint_dmp_params(self, run_time, joint_dmp_info, initial_senso...
method _check_dmp_info_parameters (line 457) | def _check_dmp_info_parameters(self, dmp_info):
method add_pose_dmp_params (line 481) | def add_pose_dmp_params(self, orientation_only, position_only, ee_fram...
method add_quaternion_pose_dmp_params (line 519) | def add_quaternion_pose_dmp_params(self, ee_frame, run_time, position_...
method add_run_time (line 563) | def add_run_time(self, run_time):
method create_goal (line 572) | def create_goal(self):
method feedback_callback (line 596) | def feedback_callback(self, feedback):
FILE: frankapy/utils.py
function franka_pose_to_rigid_transform (line 6) | def franka_pose_to_rigid_transform(franka_pose, from_frame='franka_tool_...
function convert_rigid_transform_to_array (line 17) | def convert_rigid_transform_to_array(rigid_tf):
function convert_array_to_rigid_transform (line 21) | def convert_array_to_rigid_transform(tf_array, from_frame='franka_tool',...
function min_jerk_weight (line 36) | def min_jerk_weight(t, T):
function min_jerk (line 42) | def min_jerk(xi, xf, t, T):
function min_jerk_delta (line 47) | def min_jerk_delta(xi, xf, t, T, dt):
function transform_to_list (line 52) | def transform_to_list(T):
FILE: scripts/record_trajectory.py
function create_formated_skill_dict (line 10) | def create_formated_skill_dict(joints, end_effector_positions, time_sinc...
Condensed preview — 75 files, each showing path, character count, and a content snippet. Download the .json file or copy for the full structured content (296K chars).
[
{
"path": ".github/workflows/docs_pages_workflow.yml",
"chars": 804,
"preview": "name: docs_pages_workflow\n \n# execute this workflow automatically when a we push to master\non:\n push:\n branches: [ m"
},
{
"path": ".gitignore",
"chars": 1839,
"preview": "# Byte-compiled / optimized / DLL files\n__pycache__/\n*.py[cod]\n*$py.class\n\n# C extensions\n*.so\n\n# Distribution / packagi"
},
{
"path": ".gitmodules",
"chars": 157,
"preview": "[submodule \"catkin_ws/src/franka-interface-msgs\"]\n\tpath = catkin_ws/src/franka-interface-msgs\n\turl = https://github.com/"
},
{
"path": "LICENSE",
"chars": 1083,
"preview": "MIT License\n\nCopyright (c) 2021 Carnegie Mellon University\n\nPermission is hereby granted, free of charge, to any person "
},
{
"path": "README.md",
"chars": 7684,
"preview": "# frankapy\n\nThis is a software package used for controlling and learning skills on the Franka Emika Panda Research Robot"
},
{
"path": "bash_scripts/make_catkin.sh",
"chars": 31,
"preview": "cd catkin_ws\ncatkin build\ncd .."
},
{
"path": "bash_scripts/make_proto.sh",
"chars": 77,
"preview": "protoc -I=frankapy/proto/ --python_out=frankapy/proto/ frankapy/proto/*.proto"
},
{
"path": "bash_scripts/start_control_pc.sh",
"chars": 4689,
"preview": "#!/bin/bash\n\ndie () {\n echo >&2 \"$@\"\n exit 1\n}\n\nusage=\"$(basename \"$0\") [-h] [-i xxx.xxx.xxx.xxx] -- Start control"
},
{
"path": "bash_scripts/start_franka_gripper_on_control_pc.sh",
"chars": 1292,
"preview": "#!/bin/bash\n\ncontrol_pc_uname=${1}\ncontrol_pc_ip_address=${2}\nworkstation_ip_address=${3}\ncontrol_pc_franka_interface_pa"
},
{
"path": "bash_scripts/start_franka_interface_on_control_pc.sh",
"chars": 1171,
"preview": "#!/bin/bash\n\nrobot_ip=${1}\nwith_gripper=${2}\nlog_on_franka_interface=${3}\nstop_on_error=${4}\ncontrol_pc_uname=${5}\ncontr"
},
{
"path": "bash_scripts/start_franka_ros_interface_on_control_pc.sh",
"chars": 1237,
"preview": "#!/bin/bash\n\ncontrol_pc_uname=${1}\ncontrol_pc_ip_address=${2}\nworkstation_ip_address=${3}\ncontrol_pc_franka_interface_pa"
},
{
"path": "bash_scripts/start_rosmaster.sh",
"chars": 20,
"preview": "#!/bin/bash\n\nroscore"
},
{
"path": "cfg/april_tag_pick_place_cfg.yaml",
"chars": 455,
"preview": "rs:\n id: 0\n frame: realsense\n filter_depth: True\n\napril_tag:\n detector:\n families: tag36h11\n border: 1\n nth"
},
{
"path": "docs/.gitignore",
"chars": 24,
"preview": "*.swp\n/_build\n/doctrees\n"
},
{
"path": "docs/Makefile",
"chars": 634,
"preview": "# Minimal makefile for Sphinx documentation\n#\n\n# You can set these variables from the command line, and also\n# from the "
},
{
"path": "docs/buildDocs.sh",
"chars": 3157,
"preview": "#!/bin/bash\nset -x\n################################################################################\n# File: buildDocs"
},
{
"path": "docs/conf.py",
"chars": 2912,
"preview": "# Configuration file for the Sphinx documentation builder.\n#\n# This file only contains a selection of the most common op"
},
{
"path": "docs/frankaarm.rst",
"chars": 87,
"preview": "Frankapy\n========\n\nFrankaArm\n---------\n\n.. autoclass:: frankapy.FrankaArm\n :members:\n"
},
{
"path": "docs/index.rst",
"chars": 1165,
"preview": ".. frankapy documentation master file, created by\n sphinx-quickstart on Sat Jun 5 14:43:41 2021.\n You can adapt thi"
},
{
"path": "docs/install.rst",
"chars": 2616,
"preview": "Installation\n============\n\nRequirements\n------------\n\n* A computer with Ubuntu 18.04 / 20.04\n* ROS Melodic / Noetic\n\n\nSt"
},
{
"path": "docs/make.bat",
"chars": 795,
"preview": "@ECHO OFF\r\n\r\npushd %~dp0\r\n\r\nREM Command file for Sphinx documentation\r\n\r\nif \"%SPHINXBUILD%\" == \"\" (\r\n\tset SPHINXBUILD=sp"
},
{
"path": "docs/network.rst",
"chars": 4001,
"preview": "Network Configuration\n=====================\n\nIf you are running franka-interface and frankapy on the same computer, you "
},
{
"path": "docs/running.rst",
"chars": 2280,
"preview": "Running the Robot\n=================\n\nUnlocking the Franka Robot\n--------------------------\n\n1. If you are running franka"
},
{
"path": "docs/support.rst",
"chars": 172,
"preview": "Support\n=======\n\nThe easiest way to get help with the library is to join the FrankaPy and Franka-Interface Discord using"
},
{
"path": "examples/T_RSD415_franka.tf",
"chars": 135,
"preview": "realsense\nfranka_tool\n-0.046490 0.030720 -0.083270\n-0.000000 1.000000 0.000000\n-1.000000 -0.000000 0.000000\n0.000000 0.0"
},
{
"path": "examples/T_RS_mount_delta.tf",
"chars": 68,
"preview": "franka_tool\nfranka_tool_base\n0. 0. -0.005\n1. 0. 0.\n0. 1. 0.\n0. 0. 1."
},
{
"path": "examples/april_tag_pick_place.py",
"chars": 3881,
"preview": "import os\nimport logging\nimport argparse\nfrom time import sleep\n\nimport numpy as np\n\nfrom autolab_core import RigidTrans"
},
{
"path": "examples/example_movements.py",
"chars": 1474,
"preview": "from frankapy import FrankaArm\nimport numpy as np\nfrom autolab_core import RigidTransform\n\nif __name__ == '__main__':\n\n "
},
{
"path": "examples/execute_joint_torques.py",
"chars": 476,
"preview": "import argparse\nfrom frankapy import FrankaArm\n\nif __name__ == '__main__':\n parser = argparse.ArgumentParser()\n pa"
},
{
"path": "examples/go_to_joints_with_joint_impedances.py",
"chars": 1308,
"preview": "import argparse\nimport sys\nfrom frankapy import FrankaArm\nfrom frankapy import FrankaConstants as FC\n\ndef wait_for_enter"
},
{
"path": "examples/move_robot.py",
"chars": 1549,
"preview": "import numpy as np\nfrom autolab_core import RigidTransform\n\nfrom frankapy import FrankaArm\n\n\nif __name__ == \"__main__\":\n"
},
{
"path": "examples/record_trajectory.py",
"chars": 791,
"preview": "import argparse\nimport time\nfrom frankapy import FrankaArm\nimport pickle as pkl\n\nif __name__ == '__main__':\n parser ="
},
{
"path": "examples/run_dynamic_cartesian_velocities.py",
"chars": 2462,
"preview": "import numpy as np\nimport time\n\nfrom frankapy import FrankaConstants as FC\nfrom frankapy import FrankaArm, SensorDataMes"
},
{
"path": "examples/run_dynamic_joint_velocities.py",
"chars": 2365,
"preview": "import numpy as np\nimport time\n\nfrom frankapy import FrankaConstants as FC\nfrom frankapy import FrankaArm, SensorDataMes"
},
{
"path": "examples/run_dynamic_joints.py",
"chars": 2155,
"preview": "import numpy as np\n\nfrom frankapy import FrankaArm, SensorDataMessageType\nfrom frankapy import FrankaConstants as FC\nfro"
},
{
"path": "examples/run_dynamic_pose.py",
"chars": 3177,
"preview": "import numpy as np\n\nfrom autolab_core import RigidTransform\nfrom frankapy import FrankaArm, SensorDataMessageType\nfrom f"
},
{
"path": "examples/run_hfpc.py",
"chars": 4720,
"preview": "import numpy as np\n\nfrom frankapy import FrankaArm, SensorDataMessageType\nfrom frankapy import FrankaConstants as FC\nfro"
},
{
"path": "examples/run_pose_dmp.py",
"chars": 530,
"preview": "import numpy as np\nimport math\nimport rospy\nimport argparse\nimport pickle\nfrom autolab_core import RigidTransform, Point"
},
{
"path": "examples/run_quaternion_pose_dmp.py",
"chars": 1375,
"preview": "import numpy as np\nimport math\nimport rospy\nimport argparse\nimport pickle\nfrom autolab_core import RigidTransform, Point"
},
{
"path": "examples/run_recorded_trajectory.py",
"chars": 2255,
"preview": "import pickle as pkl\nimport numpy as np\n\nfrom autolab_core import RigidTransform\nfrom frankapy import FrankaArm, SensorD"
},
{
"path": "frankapy/__init__.py",
"chars": 367,
"preview": "from .franka_arm import FrankaArm\nfrom .franka_constants import FrankaConstants\nfrom .franka_arm_state_client import Fra"
},
{
"path": "frankapy/exceptions.py",
"chars": 1055,
"preview": "class FrankaArmCommException(Exception):\n ''' Communication failure. Usually occurs due to timeouts.\n '''\n def "
},
{
"path": "frankapy/franka_arm.py",
"chars": 109431,
"preview": "\"\"\"\nfranka_arm.py\n====================================\nThe core module of frankapy\n\"\"\"\n\nimport sys, signal, logging\nfrom"
},
{
"path": "frankapy/franka_arm_state_client.py",
"chars": 3843,
"preview": "import logging\n\nimport numpy as np\nimport rospy\nfrom franka_interface_msgs.srv import GetCurrentRobotStateCmd\n\nfrom .uti"
},
{
"path": "frankapy/franka_constants.py",
"chars": 7671,
"preview": "import logging\nimport math\nimport numpy as np\nfrom autolab_core import RigidTransform\n\nclass FrankaConstants:\n '''\n "
},
{
"path": "frankapy/franka_interface_common_definitions.py",
"chars": 5382,
"preview": "#######################################################################\n# "
},
{
"path": "frankapy/proto/__init__.py",
"chars": 180,
"preview": "from .feedback_controller_params_msg_pb2 import *\nfrom .sensor_msg_pb2 import *\nfrom .termination_handler_params_msg_pb2"
},
{
"path": "frankapy/proto/feedback_controller_params_msg.proto",
"chars": 1100,
"preview": "syntax = \"proto2\";\n\nmessage CartesianImpedanceFeedbackControllerMessage {\n repeated double translational_stiffnesses = "
},
{
"path": "frankapy/proto/feedback_controller_params_msg_pb2.py",
"chars": 2852,
"preview": "# -*- coding: utf-8 -*-\n# Generated by the protocol buffer compiler. DO NOT EDIT!\n# source: feedback_controller_params_"
},
{
"path": "frankapy/proto/sensor_msg.proto",
"chars": 2401,
"preview": "// franka-interface/proto/sensor_msg.proto and frankapy/protosensor_msg.proto should match\n\nsyntax = \"proto2\";\n\nmessage "
},
{
"path": "frankapy/proto/sensor_msg_pb2.py",
"chars": 4754,
"preview": "# -*- coding: utf-8 -*-\n# Generated by the protocol buffer compiler. DO NOT EDIT!\n# source: sensor_msg.proto\n\"\"\"Generat"
},
{
"path": "frankapy/proto/termination_handler_params_msg.proto",
"chars": 529,
"preview": "syntax = \"proto2\";\n\nmessage ContactTerminationHandlerMessage {\n required double buffer_time = 1;\n\n repeated double for"
},
{
"path": "frankapy/proto/termination_handler_params_msg_pb2.py",
"chars": 1842,
"preview": "# -*- coding: utf-8 -*-\n# Generated by the protocol buffer compiler. DO NOT EDIT!\n# source: termination_handler_params_"
},
{
"path": "frankapy/proto/trajectory_generator_params_msg.proto",
"chars": 2978,
"preview": "syntax = \"proto2\";\n\nmessage GripperTrajectoryGeneratorMessage {\n required bool grasp = 1;\n required double width = 2;\n"
},
{
"path": "frankapy/proto/trajectory_generator_params_msg_pb2.py",
"chars": 5506,
"preview": "# -*- coding: utf-8 -*-\n# Generated by the protocol buffer compiler. DO NOT EDIT!\n# source: trajectory_generator_params"
},
{
"path": "frankapy/proto_utils.py",
"chars": 1270,
"preview": "from franka_interface_msgs.msg import SensorData, SensorDataGroup\n\n\ndef sensor_proto2ros_msg(sensor_proto_msg, sensor_da"
},
{
"path": "frankapy/ros_utils.py",
"chars": 1568,
"preview": "import rospy\nfrom visualization_msgs.msg import Marker, MarkerArray\nimport quaternion\n\n\nclass BoxesPublisher:\n\n def _"
},
{
"path": "frankapy/skill_list.py",
"chars": 36098,
"preview": "import roslib\nroslib.load_manifest('franka_interface_msgs')\nimport rospy\nimport actionlib\nimport numpy as np\nfrom autola"
},
{
"path": "frankapy/utils.py",
"chars": 1521,
"preview": "import numpy as np \nfrom numba import jit\nfrom autolab_core import RigidTransform\n\n\ndef franka_pose_to_rigid_transform(f"
},
{
"path": "launch/franka_rviz.launch",
"chars": 356,
"preview": "<?xml version=\"1.0\" encoding=\"UTF-8\"?>\n<launch>\n\n <param name=\"robot_description\" command=\"$(find xacro)/xacro --inorde"
},
{
"path": "rviz/franka_basic_config.rviz",
"chars": 6237,
"preview": "Panels:\n - Class: rviz/Displays\n Help Height: 78\n Name: Displays\n Property Tree Widget:\n Expanded:\n "
},
{
"path": "scripts/close_gripper.py",
"chars": 103,
"preview": "from frankapy import FrankaArm\n\n\nif __name__ == '__main__':\n fa = FrankaArm()\n fa.close_gripper()"
},
{
"path": "scripts/open_gripper.py",
"chars": 102,
"preview": "from frankapy import FrankaArm\n\n\nif __name__ == '__main__':\n fa = FrankaArm()\n fa.open_gripper()"
},
{
"path": "scripts/record_trajectory.py",
"chars": 2004,
"preview": "import argparse\nimport time\nfrom frankapy import FrankaArm\nimport pickle as pkl\nimport numpy as np\n\nfrom frankapy.utils "
},
{
"path": "scripts/reset_arm.py",
"chars": 645,
"preview": "import argparse\nfrom frankapy import FrankaArm\n\nif __name__ == '__main__':\n parser = argparse.ArgumentParser()\n pa"
},
{
"path": "scripts/run_guide_mode.py",
"chars": 536,
"preview": "import argparse\nfrom frankapy import FrankaArm\n\nif __name__ == '__main__':\n parser = argparse.ArgumentParser()\n pa"
},
{
"path": "scripts/run_guide_mode_ee_frame.py",
"chars": 548,
"preview": "import argparse\nfrom frankapy import FrankaArm\n\nif __name__ == '__main__':\n parser = argparse.ArgumentParser()\n pa"
},
{
"path": "scripts/run_recorded_trajectory.py",
"chars": 3713,
"preview": "import argparse\nimport pickle\nimport numpy as np\nimport time\n\nfrom autolab_core import RigidTransform\nfrom frankapy impo"
},
{
"path": "scripts/test_virtual_walls.py",
"chars": 251,
"preview": "from frankapy import FrankaArm\n\nif __name__ == '__main__':\n print('Starting robot')\n fa = FrankaArm()\n\n fa.rese"
},
{
"path": "setup.py",
"chars": 577,
"preview": "\"\"\"\nFrankaPy Franka Panda Robot Control Library\n\"\"\"\nfrom setuptools import setup\n\nrequirements = [\n 'autolab_core',\n "
},
{
"path": "urdf/hand.urdf.xacro",
"chars": 180,
"preview": "<?xml version=\"1.0\" encoding=\"utf-8\"?>\n<robot xmlns:xacro=\"http://www.ros.org/wiki/xacro\" name=\"hand\">\n <xacro:include "
},
{
"path": "urdf/hand.xacro",
"chars": 2247,
"preview": "<?xml version=\"1.0\" encoding=\"utf-8\"?>\n<robot xmlns:xacro=\"http://www.ros.org/wiki/xacro\" name=\"hand\">\n <xacro:macro na"
},
{
"path": "urdf/panda_arm.urdf.xacro",
"chars": 216,
"preview": "<?xml version='1.0' encoding='utf-8'?>\n<robot xmlns:xacro=\"http://www.ros.org/wiki/xacro\" name=\"panda\">\n <xacro:include"
},
{
"path": "urdf/panda_arm.xacro",
"chars": 6533,
"preview": "<?xml version='1.0' encoding='utf-8'?>\n<robot xmlns:xacro=\"http://www.ros.org/wiki/xacro\" name=\"panda\">\n <xacro:macro n"
},
{
"path": "urdf/panda_arm_hand.urdf.xacro",
"chars": 363,
"preview": "<?xml version=\"1.0\" encoding=\"utf-8\"?>\n<robot xmlns:xacro=\"http://www.ros.org/wiki/xacro\" name=\"panda\">\n <xacro:include"
}
]
About this extraction
This page contains the full source code of the iamlab-cmu/frankapy GitHub repository, extracted and formatted as plain text for AI agents and large language models (LLMs). The extraction includes 75 files (275.5 KB), approximately 69.0k tokens, and a symbol index with 134 extracted functions, classes, methods, constants, and types. Use this with OpenClaw, Claude, ChatGPT, Cursor, Windsurf, or any other AI tool that accepts text input. You can copy the full output to your clipboard or download it as a .txt file.
Extracted by GitExtract — free GitHub repo to text converter for AI. Built by Nikandr Surkov.