Showing preview only (417K chars total). Download the full file or copy to clipboard to get everything.
Repository: justagist/franka_ros_interface
Branch: master
Commit: f1f3649a4b03
Files: 85
Total size: 390.0 KB
Directory structure:
gitextract_jpas59zs/
├── .github/
│ └── workflows/
│ └── auto-comment.yml
├── .gitignore
├── .travis.yml
├── CMakeLists.txt
├── LICENSE
├── NOTICE
├── README.md
├── cmake/
│ └── ClangTools.cmake
├── franka.sh
├── franka_common/
│ └── franka_core_msgs/
│ ├── CMakeLists.txt
│ ├── msg/
│ │ ├── EndPointState.msg
│ │ ├── JointCommand.msg
│ │ ├── JointControllerStates.msg
│ │ ├── JointLimits.msg
│ │ └── RobotState.msg
│ └── package.xml
├── franka_interface/
│ ├── CMakeLists.txt
│ ├── README.md
│ ├── config/
│ │ ├── basic_controllers.yaml
│ │ └── robot_config.yaml
│ ├── include/
│ │ └── franka_interface/
│ │ ├── motion_controller_interface.h
│ │ └── robot_state_controller.h
│ ├── launch/
│ │ └── interface.launch
│ ├── package.xml
│ ├── scripts/
│ │ ├── enable_robot.py
│ │ ├── move_to_neutral.py
│ │ └── simple_gripper.py
│ ├── setup.py
│ ├── src/
│ │ ├── franka_control_node.cpp
│ │ ├── franka_dataflow/
│ │ │ ├── __init__.py
│ │ │ ├── getch.py
│ │ │ └── wait_for.py
│ │ ├── franka_interface/
│ │ │ ├── __init__.py
│ │ │ ├── arm.py
│ │ │ ├── gripper.py
│ │ │ ├── robot_enable.py
│ │ │ └── robot_params.py
│ │ ├── motion_controller_interface.cpp
│ │ └── robot_state_controller.cpp
│ ├── state_controller_plugin.xml
│ └── tests/
│ ├── demo_interface.py
│ ├── demo_joint_positions_keyboard.py
│ └── test_zeroG.py
├── franka_moveit/
│ ├── CMakeLists.txt
│ ├── README.md
│ ├── config/
│ │ └── moveit_demo.rviz
│ ├── launch/
│ │ └── demo_moveit.launch
│ ├── package.xml
│ ├── scripts/
│ │ └── create_demo_planning_scene.py
│ ├── setup.py
│ ├── src/
│ │ └── franka_moveit/
│ │ ├── __init__.py
│ │ ├── extended_planning_scene_interface.py
│ │ ├── movegroup_interface.py
│ │ └── utils.py
│ └── tests/
│ ├── test_movegroup_interface.py
│ └── test_scene_interface.py
├── franka_ros_controllers/
│ ├── CMakeLists.txt
│ ├── README.md
│ ├── cfg/
│ │ └── joint_controller_params.cfg
│ ├── config/
│ │ └── ros_controllers.yaml
│ ├── controller_plugins.xml
│ ├── include/
│ │ └── franka_ros_controllers/
│ │ ├── effort_joint_impedance_controller.h
│ │ ├── effort_joint_position_controller.h
│ │ ├── effort_joint_torque_controller.h
│ │ ├── position_joint_position_controller.h
│ │ └── velocity_joint_velocity_controller.h
│ ├── package.xml
│ ├── scripts/
│ │ └── test_controller.py
│ └── src/
│ ├── effort_joint_impedance_controller.cpp
│ ├── effort_joint_position_controller.cpp
│ ├── effort_joint_torque_controller.cpp
│ ├── position_joint_position_controller.cpp
│ └── velocity_joint_velocity_controller.cpp
├── franka_ros_interface/
│ ├── CMakeLists.txt
│ └── package.xml
└── franka_tools/
├── CMakeLists.txt
├── README.md
├── package.xml
├── setup.py
└── src/
└── franka_tools/
├── __init__.py
├── collision_behaviour_interface.py
├── controller_manager_interface.py
├── controller_param_config_client.py
├── frames_interface.py
└── joint_trajectory_action_client.py
================================================
FILE CONTENTS
================================================
================================================
FILE: .github/workflows/auto-comment.yml
================================================
name: Auto Comment
on: [issues, pull_request]
jobs:
run:
runs-on: ubuntu-latest
steps:
- uses: wow-actions/auto-comment@v1
with:
GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }}
issuesOpened: |
Hi @{{ author }}, thank you for using this repository. Unfortunately, I am unable to maintain this package any more, sorry!
If you are interested in contributing to this repository or are willing to maintain it, please contact me at mail@saifsidhik.page.
-- Saif
pullRequestOpened: |
Hi @{{ author }},
Thank you for the pull request. Unfortunately, I am unable to maintain this package any more, sorry!
Please feel free to maintain a working fork of the repo. If you are interested in contributing to this repository or are willing to maintain it, please contact me at mail@saifsidhik.page.
-- Saif
================================================
FILE: .gitignore
================================================
*.py[cod]
__pycache__
*$py.class
# C extensions
*.so
# Packages
*.egg
*.egg-info
dist
build
eggs
parts
bin
var
sdist
develop-eggs
.installed.cfg
lib
lib64
# Installer logs
pip-log.txt
# Unit test / coverage reports
.coverage
.tox
nosetests.xml
# Translations
*.mo
# Mr Developer
.mr.developer.cfg
.project
.cproject
cmake_install.cmake
.pydevproject
*.swp
*.swo
CATKIN_IGNORE
catkin
catkin_generated
devel
cpp
docs_src
doc_src
docs_build
msg_gen
srv_gen
*.smoketest
*.cfgc
*_msgs/src/
*/src/**/cfg
*/*/src/**/*
franka_interface/scripts/test.py
================================================
FILE: .travis.yml
================================================
# Generic .travis.yml file for running continuous integration on Travis-CI for
# any ROS package.
#
# Available here:
# - https://github.com/felixduvallet/ros-travis-integration
#
# This installs ROS on a clean Travis-CI virtual machine, creates a ROS
# workspace, resolves all listed dependencies, and sets environment variables
# (setup.bash). Then, it compiles the entire ROS workspace (ensuring there are
# no compilation errors), and runs all the tests. If any of the compilation/test
# phases fail, the build is marked as a failure.
#
# We handle two types of package dependencies specified in the package manifest:
# - system dependencies that can be installed using `rosdep`, including other
# ROS packages and system libraries. These dependencies must be known to
# `rosdistro` and are installed using apt-get.
# - package dependencies that must be checked out from source. These are handled by
# `wstool`, and should be listed in a file named dependencies.rosinstall.
#
# There are envioronment variables you may want to change, such as ROS_DISTRO,
# ROSINSTALL_FILE, and the CATKIN_OPTIONS file. See the README.md for more
# information on these flags, and
# https://docs.travis-ci.com/user/environment-variables/ for information about
# Travis environment variables in general.
#
# Author: Felix Duvallet <felixd@gmail.com>
# NOTE: The build lifecycle on Travis.ci is something like this:
# before_install
# install
# before_script
# script
# after_success or after_failure
# after_script
# OPTIONAL before_deploy
# OPTIONAL deploy
# OPTIONAL after_deploy
################################################################################
sudo: required
cache:
- apt
# Build all valid Ubuntu/ROS combinations available on Travis VMs.
language: generic
matrix:
include:
- name: "focal noetic"
dist: focal
env: ROS_DISTRO=noetic
- name: "Bionic melodic"
dist: bionic
env: ROS_DISTRO=melodic
# Configuration variables. All variables are global now, but this can be used to
# trigger a build matrix for different ROS distributions if desired.
env:
global:
- ROS_CI_DESKTOP="`lsb_release -cs`" # e.g. [trusty|xenial|...]
- CI_SOURCE_PATH=$(pwd)
- ROSINSTALL_FILE=$CI_SOURCE_PATH/dependencies.rosinstall
- ROS_PARALLEL_JOBS='-j8 -l6'
# Set the python path manually to include /usr/-/python2.7/dist-packages
# as this is where apt-get installs python packages.
################################################################################
# Install system dependencies, namely a very barebones ROS setup.
before_install:
- if [[ $ROS_DISTRO == "noetic" ]]; then export PYMODULE=python3; else export PYMODULE=python; fi;
- export PYTHONPATH=$PYTHONPATH:/usr/lib/${PYMODULE}/dist-packages:/usr/local/lib/${PYMODULE}/dist-packages
- sudo sh -c "echo \"deb http://packages.ros.org/ros/ubuntu $ROS_CI_DESKTOP main\" > /etc/apt/sources.list.d/ros-latest.list"
- sudo apt-key adv --keyserver 'hkp://keyserver.ubuntu.com:80' --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654
- sudo apt-get update -qq
- sudo apt-get install dpkg
- sudo apt-get install -y ${PYMODULE}-catkin-pkg ${PYMODULE}-rosdep ${PYMODULE}-wstool ros-$ROS_DISTRO-desktop-full ros-$ROS_DISTRO-combined-robot-hw
- source /opt/ros/$ROS_DISTRO/setup.bash
- sudo apt install -y ros-$ROS_DISTRO-libfranka
- if [[ $ROS_DISTRO == "melodic" ]]; then sudo apt install -y ros-$ROS_DISTRO-franka-ros; fi;
- sudo apt install -y ${PYMODULE}-catkin-tools ${PYMODULE}-osrf-pycommon
# Prepare rosdep to install dependencies.
- sudo rosdep init
- rosdep update --include-eol-distros # Support EOL distros.
# Create a catkin workspace with the package under integration.
install:
- mkdir -p ~/catkin_ws/src
- cd ~/catkin_ws/src
- catkin_init_workspace
# Create the devel/setup.bash (run catkin_make with an empty workspace) and
# source it to set the path variables.
- cd ~/catkin_ws
- catkin build
- source devel/setup.bash
# Add the package under integration to the workspace using a symlink.
- cd ~/catkin_ws/src
- ln -s $CI_SOURCE_PATH .
# Install all dependencies, using wstool first and rosdep second.
# wstool looks for a ROSINSTALL_FILE defined in the environment variables.
before_script:
# source dependencies: install using wstool.
- cd ~/catkin_ws/src
- wstool init
- if [[ -f $ROSINSTALL_FILE ]] ; then wstool merge $ROSINSTALL_FILE ; fi
- wstool up
- if [[ $ROS_DISTRO == "noetic" ]]; then git clone -b ${ROS_DISTRO}-devel https://github.com/frankaemika/franka_ros; git clone -b melodic-devel https://github.com/ros-planning/panda_moveit_config; fi;
# package depdencies: install using rosdep.
- cd ~/catkin_ws
# - rosdep install -y --from-paths src --ignore-src --rosdistro $ROS_DISTRO
# Compile and test (mark the build as failed if any step fails). If the
# CATKIN_OPTIONS file exists, use it as an argument to catkin_make, for example
# to blacklist certain packages.
#
# NOTE on testing: `catkin_make run_tests` will show the output of the tests
# (gtest, nosetest, etc..) but always returns 0 (success) even if a test
# fails. Running `catkin_test_results` aggregates all the results and returns
# non-zero when a test fails (which notifies Travis the build failed).
script:
- source /opt/ros/$ROS_DISTRO/setup.bash
- cd ~/catkin_ws
# seems to need separate builds for succeeding when using franka_ros from source
- if [[ $ROS_DISTRO == "noetic" ]]; then catkin build franka_msgs; source devel/setup.bash; catkin build franka_ros; fi;
- source devel/setup.bash
# Run the tests, ensuring the path is set correctly.
- catkin build franka_ros_interface #--catkin-make-args run_tests && catkin_test_results
================================================
FILE: CMakeLists.txt
================================================
# toplevel CMakeLists.txt for a catkin workspace
# catkin/cmake/toplevel.cmake
cmake_minimum_required(VERSION 2.8.3)
set(CATKIN_TOPLEVEL TRUE)
# search for catkin within the workspace
set(_cmd "catkin_find_pkg" "catkin" "${CMAKE_SOURCE_DIR}")
execute_process(COMMAND ${_cmd}
RESULT_VARIABLE _res
OUTPUT_VARIABLE _out
ERROR_VARIABLE _err
OUTPUT_STRIP_TRAILING_WHITESPACE
ERROR_STRIP_TRAILING_WHITESPACE
)
if(NOT _res EQUAL 0 AND NOT _res EQUAL 2)
# searching fot catkin resulted in an error
string(REPLACE ";" " " _cmd_str "${_cmd}")
message(FATAL_ERROR "Search for 'catkin' in workspace failed (${_cmd_str}): ${_err}")
endif()
# include catkin from workspace or via find_package()
if(_res EQUAL 0)
set(catkin_EXTRAS_DIR "${CMAKE_SOURCE_DIR}/${_out}/cmake")
# include all.cmake without add_subdirectory to let it operate in same scope
include(${catkin_EXTRAS_DIR}/all.cmake NO_POLICY_SCOPE)
add_subdirectory("${_out}")
else()
# use either CMAKE_PREFIX_PATH explicitly passed to CMake as a command line argument
# or CMAKE_PREFIX_PATH from the environment
if(NOT DEFINED CMAKE_PREFIX_PATH)
if(NOT "$ENV{CMAKE_PREFIX_PATH}" STREQUAL "")
if(NOT WIN32)
string(REPLACE ":" ";" CMAKE_PREFIX_PATH $ENV{CMAKE_PREFIX_PATH})
else()
set(CMAKE_PREFIX_PATH $ENV{CMAKE_PREFIX_PATH})
endif()
endif()
endif()
# list of catkin workspaces
set(catkin_search_path "")
foreach(path ${CMAKE_PREFIX_PATH})
if(EXISTS "${path}/.catkin")
list(FIND catkin_search_path ${path} _index)
if(_index EQUAL -1)
list(APPEND catkin_search_path ${path})
endif()
endif()
endforeach()
# search for catkin in all workspaces
set(CATKIN_TOPLEVEL_FIND_PACKAGE TRUE)
find_package(catkin QUIET
NO_POLICY_SCOPE
PATHS ${catkin_search_path}
NO_DEFAULT_PATH NO_CMAKE_FIND_ROOT_PATH)
unset(CATKIN_TOPLEVEL_FIND_PACKAGE)
if(NOT catkin_FOUND)
message(FATAL_ERROR "find_package(catkin) failed. catkin was neither found in the workspace nor in the CMAKE_PREFIX_PATH. One reason may be that no ROS setup.sh was sourced before.")
endif()
endif()
catkin_workspace()
================================================
FILE: LICENSE
================================================
Apache License
Version 2.0, January 2004
http://www.apache.org/licenses/
TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
1. Definitions.
"License" shall mean the terms and conditions for use, reproduction,
and distribution as defined by Sections 1 through 9 of this document.
"Licensor" shall mean the copyright owner or entity authorized by
the copyright owner that is granting the License.
"Legal Entity" shall mean the union of the acting entity and all
other entities that control, are controlled by, or are under common
control with that entity. For the purposes of this definition,
"control" means (i) the power, direct or indirect, to cause the
direction or management of such entity, whether by contract or
otherwise, or (ii) ownership of fifty percent (50%) or more of the
outstanding shares, or (iii) beneficial ownership of such entity.
"You" (or "Your") shall mean an individual or Legal Entity
exercising permissions granted by this License.
"Source" form shall mean the preferred form for making modifications,
including but not limited to software source code, documentation
source, and configuration files.
"Object" form shall mean any form resulting from mechanical
transformation or translation of a Source form, including but
not limited to compiled object code, generated documentation,
and conversions to other media types.
"Work" shall mean the work of authorship, whether in Source or
Object form, made available under the License, as indicated by a
copyright notice that is included in or attached to the work
(an example is provided in the Appendix below).
"Derivative Works" shall mean any work, whether in Source or Object
form, that is based on (or derived from) the Work and for which the
editorial revisions, annotations, elaborations, or other modifications
represent, as a whole, an original work of authorship. For the purposes
of this License, Derivative Works shall not include works that remain
separable from, or merely link (or bind by name) to the interfaces of,
the Work and Derivative Works thereof.
"Contribution" shall mean any work of authorship, including
the original version of the Work and any modifications or additions
to that Work or Derivative Works thereof, that is intentionally
submitted to Licensor for inclusion in the Work by the copyright owner
or by an individual or Legal Entity authorized to submit on behalf of
the copyright owner. For the purposes of this definition, "submitted"
means any form of electronic, verbal, or written communication sent
to the Licensor or its representatives, including but not limited to
communication on electronic mailing lists, source code control systems,
and issue tracking systems that are managed by, or on behalf of, the
Licensor for the purpose of discussing and improving the Work, but
excluding communication that is conspicuously marked or otherwise
designated in writing by the copyright owner as "Not a Contribution."
"Contributor" shall mean Licensor and any individual or Legal Entity
on behalf of whom a Contribution has been received by Licensor and
subsequently incorporated within the Work.
2. Grant of Copyright License. Subject to the terms and conditions of
this License, each Contributor hereby grants to You a perpetual,
worldwide, non-exclusive, no-charge, royalty-free, irrevocable
copyright license to reproduce, prepare Derivative Works of,
publicly display, publicly perform, sublicense, and distribute the
Work and such Derivative Works in Source or Object form.
3. Grant of Patent License. Subject to the terms and conditions of
this License, each Contributor hereby grants to You a perpetual,
worldwide, non-exclusive, no-charge, royalty-free, irrevocable
(except as stated in this section) patent license to make, have made,
use, offer to sell, sell, import, and otherwise transfer the Work,
where such license applies only to those patent claims licensable
by such Contributor that are necessarily infringed by their
Contribution(s) alone or by combination of their Contribution(s)
with the Work to which such Contribution(s) was submitted. If You
institute patent litigation against any entity (including a
cross-claim or counterclaim in a lawsuit) alleging that the Work
or a Contribution incorporated within the Work constitutes direct
or contributory patent infringement, then any patent licenses
granted to You under this License for that Work shall terminate
as of the date such litigation is filed.
4. Redistribution. You may reproduce and distribute copies of the
Work or Derivative Works thereof in any medium, with or without
modifications, and in Source or Object form, provided that You
meet the following conditions:
(a) You must give any other recipients of the Work or
Derivative Works a copy of this License; and
(b) You must cause any modified files to carry prominent notices
stating that You changed the files; and
(c) You must retain, in the Source form of any Derivative Works
that You distribute, all copyright, patent, trademark, and
attribution notices from the Source form of the Work,
excluding those notices that do not pertain to any part of
the Derivative Works; and
(d) If the Work includes a "NOTICE" text file as part of its
distribution, then any Derivative Works that You distribute must
include a readable copy of the attribution notices contained
within such NOTICE file, excluding those notices that do not
pertain to any part of the Derivative Works, in at least one
of the following places: within a NOTICE text file distributed
as part of the Derivative Works; within the Source form or
documentation, if provided along with the Derivative Works; or,
within a display generated by the Derivative Works, if and
wherever such third-party notices normally appear. The contents
of the NOTICE file are for informational purposes only and
do not modify the License. You may add Your own attribution
notices within Derivative Works that You distribute, alongside
or as an addendum to the NOTICE text from the Work, provided
that such additional attribution notices cannot be construed
as modifying the License.
You may add Your own copyright statement to Your modifications and
may provide additional or different license terms and conditions
for use, reproduction, or distribution of Your modifications, or
for any such Derivative Works as a whole, provided Your use,
reproduction, and distribution of the Work otherwise complies with
the conditions stated in this License.
5. Submission of Contributions. Unless You explicitly state otherwise,
any Contribution intentionally submitted for inclusion in the Work
by You to the Licensor shall be under the terms and conditions of
this License, without any additional terms or conditions.
Notwithstanding the above, nothing herein shall supersede or modify
the terms of any separate license agreement you may have executed
with Licensor regarding such Contributions.
6. Trademarks. This License does not grant permission to use the trade
names, trademarks, service marks, or product names of the Licensor,
except as required for reasonable and customary use in describing the
origin of the Work and reproducing the content of the NOTICE file.
7. Disclaimer of Warranty. Unless required by applicable law or
agreed to in writing, Licensor provides the Work (and each
Contributor provides its Contributions) on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
implied, including, without limitation, any warranties or conditions
of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
PARTICULAR PURPOSE. You are solely responsible for determining the
appropriateness of using or redistributing the Work and assume any
risks associated with Your exercise of permissions under this License.
8. Limitation of Liability. In no event and under no legal theory,
whether in tort (including negligence), contract, or otherwise,
unless required by applicable law (such as deliberate and grossly
negligent acts) or agreed to in writing, shall any Contributor be
liable to You for damages, including any direct, indirect, special,
incidental, or consequential damages of any character arising as a
result of this License or out of the use or inability to use the
Work (including but not limited to damages for loss of goodwill,
work stoppage, computer failure or malfunction, or any and all
other commercial damages or losses), even if such Contributor
has been advised of the possibility of such damages.
9. Accepting Warranty or Additional Liability. While redistributing
the Work or Derivative Works thereof, You may choose to offer,
and charge a fee for, acceptance of support, warranty, indemnity,
or other liability obligations and/or rights consistent with this
License. However, in accepting such obligations, You may act only
on Your own behalf and on Your sole responsibility, not on behalf
of any other Contributor, and only if You agree to indemnify,
defend, and hold each Contributor harmless for any liability
incurred by, or claims asserted against, such Contributor by reason
of your accepting any such warranty or additional liability.
END OF TERMS AND CONDITIONS
APPENDIX: How to apply the Apache License to your work.
To apply the Apache License to your work, attach the following
boilerplate notice, with the fields enclosed by brackets "[]"
replaced with your own identifying information. (Don't include
the brackets!) The text should be enclosed in the appropriate
comment syntax for the file format. We also recommend that a
file or class name and description of purpose be included on the
same "printed page" as the copyright notice for easier
identification within third-party archives.
Copyright 2019-2021 Saif Sidhik
Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at
http://www.apache.org/licenses/LICENSE-2.0
Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
================================================
FILE: NOTICE
================================================
franka_ros_interface package
Copyright 2019-2021 Saif Sidhik
Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at
http://www.apache.org/licenses/LICENSE-2.0
Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
================================================
FILE: README.md
================================================
# Franka ROS Interface [](https://github.com/justagist/franka_ros_interface/tree/v0.7.1-dev) [](https://ros.org/) [](https://www.python.org/downloads/release/python-360/)
[](https://www.codacy.com/manual/justagist/franka_ros_interface?utm_source=github.com&utm_medium=referral&utm_content=justagist/franka_ros_interface&utm_campaign=Badge_Grade)
[](https://travis-ci.com/justagist/franka_ros_interface)
A ROS interface library for the Franka Emika Panda robot (**_real and [simulated][ps-repo]_**), extending the [franka-ros][franka-ros] library to expose more information about the robot, and
providing low-level control of the robot using ROS and [Python API][fri-doc].
Franka ROS Interface provides utilites for controlling and managing the Franka Emika Panda robot. Contains exposed customisable controllers for the robot (joint position,
velocity, torque), interfaces for the gripper, controller manager, coordinate frames interface, etc. Also provides utilities to control the robot using 'MoveIt!' and ROS Trajectory Action & ActionClient. This package also provides almost complete sim-to-real / real-to-sim transfer of code with the [Panda Simulator][ps-repo] package.
Documentation Page: <https://justagist.github.io/franka_ros_interface>
**This branch requires franka_ros release version 0.7.1** [](https://github.com/frankaemika/franka_ros/tree/49e5ac1055e332581b4520a1bd9ac8aaf4580fb1). (For older franka_ros versions, try building this package from the corresponding branches of this repo. All functionalities may not be available in older versions.)
A more unified ROS Python interface built over this package is available at [PandaRobot](https://github.com/justagist/panda_robot), which provides a more intuitive interface class that combines the different API classes in this package. Simple demos are also available.
## Features
- Low-level *controllers* (joint position, velocity, torque, impedance) available that can be controlled through ROS topics and Python API (including position control for gripper).
- Real-time *robot state* (end-effector state, joint state, controller state, etc.) available through ROS topics and Python API.
- Python API for managing controllers, coordinate frames, collision behaviour, controlling and monitoring the gripper.
- Python API classes and utility functions to control the robot using MoveIt! and ROS Trajectory Action Service.
- The [panda_simulator][ps-repo] package (which is Gazebo-based simulator for the robot) can also be controlled using this package (ROS and Python interface), providing almost complete sim-to-real transfer of code.
**Demo Using [PandaRobot API](https://github.com/justagist/panda_robot) and [Panda Simulator][ps-repo]**
Watch video [here](https://youtu.be/4bEVysUIvOY)
Watch video [here](https://www.youtube.com/watch?v=NdSbXC0r7tU)
Watch video [here](https://youtu.be/a_HEmYzqEnk)
## Installation
This branch works with ROS Melodic and ROS Noetic. [](https://travis-ci.com/justagist/franka_ros_interface)
**NOTE:** Tested on:
| ROS Version | Python Version | Franka ROS Branch |
|-------------|----------------|-------------------------------------------------------------------------------|
| Melodic | 2.7+ | [melodic-devel](https://github.com/frankaemika/franka_ros/tree/melodic-devel) |
| Noetic | 3.6+ | [noetic-devel](https://github.com/frankaemika/franka_ros/tree/noetic-devel) |
### Dependencies
- ROS Melodic / Noetic (preferably the 'desktop-full' version)
- *libfranka (required version >= 0.8.0)* (`sudo apt install ros-$ROS_DISTRO-libfranka` or [install from source][libfranka-doc]). *Use the release version if building from source for reliable build.*
- *franka-ros* v0.7.1 (`sudo apt install ros-$ROS_DISTRO-franka-ros` or [install from source][franka-ros]). *Make sure to use the appropriate branch of franka_ros (`melodic-devel` or `noetic-devel`) depending on your ROS version.*
- *panda_moveit_config* (`sudo apt install ros-$ROS_DISTRO-panda-moveit-config` or [install from source](https://github.com/ros-planning/panda_moveit_config) (the 'melodic-devel' branch also works for ROS Noetic))
- [*franka_panda_description*][fpd-repo] (Clone this repository to the `src` folder of your workspace. See [Related Packages](#related-packages) section for information about package). **NOTE**: Installing this package is optional, but recommended. If you do not want to use the *franka_panda_description* package, make sure you modify the `franka_interface/launch/interface.launch` file and replace all occurences of `franka_panda_description` with `franka_description` (two occurences).
Once the above dependencies are installed, the package can be installed using catkin:
```sh
cd <catkin_ws>
git clone -b v0.7.1-dev https://github.com/justagist/franka_ros_interface src/franka_ros_interface
catkin build # or catkin_make (catkin build is recommended)
source devel/setup.bash
```
*Note: Python code in this package is written to be compatible with both Python 2 and 3, so make sure you have the Python `future` module installed (`pip install future` (or `pip3 install ..`)).*
After building the package (this is not required if using with simulated robot):
- Copy/move the *franka.sh* file to the root of the catkin_ws `$ cp src/franka_ros_interface/franka.sh ./`
- Change the values in the copied file (described in the file).
## Usage
**NOTE: For using this package with [Panda Simulator][ps-repo], the following sections are not required; all the required "driver" nodes are started along with the simulation launch file, and the Franka ROS Interface API (as well as [PandaRobot](https://github.com/justagist/panda_robot) API) can be directly used. Follow instructions in the [demos](https://github.com/justagist/panda_simulator#demos) section in the Panda Simulator package. See their corresponding [source files](https://github.com/justagist/panda_simulator/tree/melodic-devel/panda_simulator_examples/scripts) for usage examples.**
### The *franka.sh* environments
Once the values are correctly modified in the `franka.sh` file, different environments can be set for controlling the robot by sourcing this file.
- For instance, running `./franka.sh master` would start an environment assuming that the computer is directly connected to the robot (requires Real-Time kernel set up as described in the [FCI documentation][libfranka-doc]).
- On the other hand, `./franka.sh remote` would start an environment assuming that the robot is not connected directly to the computer, but to another computer in the network (whose IP must be specified in *franka.sh*). This way, if the 'master' is connected to the robot and running the driver node (see below), the 'remote' can control the robot (**_no need for Real Time kernel!_**) as long as they are in the same network.
- ~Simulation environment can be started by running `./franka.sh sim` (only required when using [panda_simulator][ps-repo] package).~ *Not required anymore*
More information regarding the usage of `franka.sh` can be found within the [franka.sh file](franka.sh).
### Starting the Franka ROS Interface 'Driver'
The 'driver' node can be started by running (can only be used if run in 'master' environment - see [Environments](#the-frankash-environments) section above):
```sh
roslaunch franka_interface interface.launch # (use argument load_gripper:=false for starting without gripper)
```
Available keyword arguments for launch file:
- `load_gripper`: start driver node with the Franka gripper (default: `true`).
- `start_controllers`: load the available controllers to the controller manager (default: `true`).
- `start_moveit`: start moveit server along with the driver node (default: `true`).
- `load_demo_planning_scene`: loads a default planning scene for MoveIt planning with simple objects for collision avoidance (default: `true`). See [create_demo_planning_scene.py](franka_moveit/scripts/create_demo_planning_scene.py).
This starts the robot controllers and drivers to expose a variety of ROS topics and services for communicating with and controlling the robot.
### Controlling and Monitoring the Robot
Once the 'driver' is running, the robot can be controlled from another terminal by running in 'master' environment (if running in the same machine as 'driver'), or 'remote' environment (if using a different connected computer). The robot can then be controlled and monitored using ROS topics and services (see below to find out about some of the available topics and services), or using the provided [Python API][fri-doc] (also see [*PandaRobot*](https://github.com/justagist/panda_robot)).
Example of the robot working with MoveIt can be found by running `roslaunch franka_moveit demo_moveit.launch`.
Basic usage of the Python API is shown in the [`demo_interface.py` example file](franka_interface/tests/demo_interface.py). An example of controlling the robot joints with keyboard using direct position control is shown in [`demo_joint_positions_keyboard.py`](https://github.com/justagist/franka_ros_interface/blob/master/franka_interface/tests/demo_joint_positions_keyboard.py) (read the notes in the file before running the demo. start this demo by running `rosrun franka_interface joint_position_keyboard.py`).
See [documentation][fri-doc] for all available methods and functionalities.
See the [tests](https://github.com/justagist/franka_ros_interface/tree/master/franka_interface/tests) and [scripts](https://github.com/justagist/franka_ros_interface/tree/master/franka_interface/scripts) directories in `franka_interface` for few more usage examples.
**More usage examples can be found in the [PandaRobot](https://github.com/justagist/panda_robot) package.**
#### Some useful ROS topics
##### Published Topics
| ROS Topic | Data |
| ------ | ------ |
| */franka_ros_interface/custom_franka_state_controller/robot_state* | gravity, coriolis, jacobian, cartesian velocity, etc. |
| */franka_ros_interface/custom_franka_state_controller/tip_state* | end-effector pose, wrench, etc. |
| */franka_ros_interface/joint_states* | joint positions, velocities, efforts |
| */franka_ros_interface/franka_gripper/joint_states* | joint positions, velocities, efforts of gripper joints |
##### Subscribed Topics
| ROS Topic | Data |
| ------ | ------ |
| */franka_ros_interface/motion_controller/arm/joint_commands* | command the robot using the currently active controller |
| */franka_ros_interface/franka_gripper/[move/grasp/stop/homing]* | (action msg) command the joints of the gripper |
Other topics for changing the controller gains (also dynamically configurable), command timeout, etc. are also available.
#### ROS Services
Controller manager service can be used to switch between all available controllers (joint position, velocity, effort). Gripper joints can be controlled using the ROS ActionClient. Other services for changing coordinate frames, adding gripper load configuration, etc. are also available.
#### Python API
Most of the above services and topics are wrapped using simple Python classes or utility functions, providing more control and simplicity. This includes direct control of the robot and gripper using the provided custom low-level controllers, MoveIt, and JointTrajectoryAction. Refer README files in individual subpackages.
[Documentation][fri-doc]
More usage examples in the [PandaRobot](https://github.com/justagist/panda_robot) package.
## Related Packages
- [*panda_simulator*][ps-repo] : A Gazebo simulator for the Franka Emika Panda robot with ROS interface, providing exposed controllers and real-time robot state feedback similar to the real robot when using the *franka_ros_interface* package. Provides almost complete real-to-sim transfer of code.
- [*PandaRobot*](https://github.com/justagist/panda_robot) : Python interface providing higher-level control of the robot integrated with its gripper control, controller manager, coordinate frames manager, etc. with safety checks and other helper utilities. It also provides the kinematics and dynamics of the robot using the [KDL library](http://wiki.ros.org/kdl). It is built over Franka ROS Interface and provides a more intuitive and unified single-class interface.
- [*franka_panda_description*][fpd-repo] : Robot description package modified from [*franka_ros*][franka-ros] package to include dynamics parameters for the robot arm (as estimated in [this paper](https://hal.inria.fr/hal-02265293/document)). Also includes transmission and control definitions required for the [*panda_simulator*][ps-repo] package.
[ps-repo]: <https://github.com/justagist/panda_simulator>
[fri-repo]: <https://github.com/justagist/franka_ros_interface>
[fpd-repo]: <https://github.com/justagist/franka_panda_description>
[fri-doc]: <https://justagist.github.io/franka_ros_interface>
[libfranka-doc]: <https://frankaemika.github.io/docs/installation_linux.html#building-from-source>
[franka-ros]: <https://github.com/frankaemika/franka_ros>
## License
[](https://opensource.org/licenses/Apache-2.0)
Copyright (c) 2019-2021, Saif Sidhik
If you use this software for research, please considering citing using [](https://doi.org/10.5281/zenodo.3747412).
================================================
FILE: cmake/ClangTools.cmake
================================================
include(CMakeParseArguments)
find_program(CLANG_FORMAT_PROG clang-format-5.0 DOC "'clang-format' executable")
if(CLANG_FORMAT_PROG AND NOT TARGET format)
add_custom_target(format)
add_custom_target(check-format)
endif()
find_program(CLANG_TIDY_PROG clang-tidy-5.0 DOC "'clang-tidy' executable")
if(CLANG_TIDY_PROG AND NOT TARGET tidy)
if(NOT CMAKE_EXPORT_COMPILE_COMMANDS)
message(WARNING "Invoke Catkin/CMake with '-DCMAKE_EXPORT_COMPILE_COMMANDS=ON'
to generate compilation database for 'clang-tidy'.")
endif()
add_custom_target(tidy)
add_custom_target(check-tidy)
endif()
function(add_format_target _target)
if(NOT CLANG_FORMAT_PROG)
return()
endif()
cmake_parse_arguments(ARG "" "" "FILES" ${ARGN})
add_custom_target(format-${_target}
COMMAND ${CLANG_FORMAT_PROG} -i ${ARG_FILES}
WORKING_DIRECTORY ${CMAKE_CURRENT_LIST_DIR}/..
COMMENT "Formatting ${_target} source code with clang-format"
VERBATIM
)
add_dependencies(format format-${_target})
add_custom_target(check-format-${_target}
COMMAND scripts/format-check.sh ${CLANG_FORMAT_PROG} -output-replacements-xml ${ARG_FILES}
WORKING_DIRECTORY ${CMAKE_CURRENT_LIST_DIR}/..
COMMENT "Checking ${_target} code formatting with clang-format"
VERBATIM
)
add_dependencies(check-format check-format-${_target})
endfunction()
function(add_tidy_target _target)
if(NOT CLANG_TIDY_PROG)
return()
endif()
cmake_parse_arguments(ARG "" "" "FILES;DEPENDS" ${ARGN})
add_custom_target(tidy-${_target}
COMMAND ${CLANG_TIDY_PROG} -fix -p=${CMAKE_BINARY_DIR} ${ARG_FILES}
WORKING_DIRECTORY ${CMAKE_CURRENT_LIST_DIR}/..
DEPENDS ${ARG_DEPENDS}
COMMENT "Running clang-tidy for ${_target}"
VERBATIM
)
add_dependencies(tidy tidy-${_target})
add_custom_target(check-tidy-${_target}
COMMAND scripts/fail-on-output.sh ${CLANG_TIDY_PROG} -p=${CMAKE_BINARY_DIR} ${ARG_FILES}
WORKING_DIRECTORY ${CMAKE_CURRENT_LIST_DIR}/..
DEPENDS ${ARG_DEPENDS}
COMMENT "Running clang-tidy for ${_target}"
VERBATIM
)
add_dependencies(check-tidy check-tidy-${_target})
endfunction()
================================================
FILE: franka.sh
================================================
#!/bin/bash
# Copyright (c) 2019-2021, Saif Sidhik
# Copyright (c) 2013-2016, Rethink Robotics
# All rights reserved.
# This is a convenient script which will set up your ROS environment and
# should be executed with every new instance of a shell in which you plan on
# working with Franka ROS Interface or PandaRobot.
# This file is to be used in the *root* of your catkin workspace.
# ****** IMPORTANT: Read "USAGE EXPLAINED" section below before running this script. ******
# Run as './franka.sh <arg1> <arg2>'
# ------ ARGUMENTS:
# -- arg1 (required): select the environment for Franka ROS Interface. The available options are:
# - 'master' / 'local' (No <arg2>): This can be used only if this machine can succesfully connect to the robot using libfranka. By setting this, this machine will be assumed to be the ros master. The 'driver' node can only be run in this environment (see Usage instructions in readme).
# - 'slave' / 'remote': This argument should be used if the current machine cannot directly to franka controller, but is in the same network as the machine that is running the 'master' (and the 'driver' nodes). If IP address is provided as <arg2>, this will connected to a ROS Master at that IP. If <arg2> is not provided, ROS_MASTER_IP variable set below will be used.
# - 'sim': start in simulation environment (to be used with panda_simulator package only). [Not necessary]
# -- arg2 (optional):
# - <IP of ROS MASTER> (valid only if <arg1> is 'slave'/'remote'): IP address of ROS Master
# eg:
# ./franka.sh master
# (or)
# ./franka.sh remote [optional IP address of master]
## ========================================= #
## === EDIT FOLLOWING VALUES AS REQUIRED === #
## ========================================= #
# ----- EDIT THIS TO MATCH THE IP OF THE FRANKA ROBOT CONTROLLER IN THE NETWORK
FRANKA_ROBOT_IP="FRANKA_ROBOT_IP.local"
# ----- EDIT THIS TO MATCH YOUR IP IN THE NETWORK
your_ip=""
# ----- EDIT THIS TO MATCH THE IP OF THE MAIN ROS MASTER PC (CONNECTED TO THE FRANKA CONTROLLER),
# ----- ON WHICH THE Franka ROS Interface 'driver' NODES WILL BE RUNNING (SEE "USAGE EXPLAINED" BELOW).
# ----- THIS VALUE IS NEEDED ONLY IF YOU WILL USE THE 'remote' ENVIRONMENT ON THIS MACHINE.
ROS_MASTER_IP=""
ros_version="melodic"
## ========================================= #
## ========== USAGE EXPLAINED ============== #
## ========================================= #
# 1. If there is only one computer controlling the robot, there is no need
# to fill the $ROS_MASTER_IP variable in the field above. However, this
# machine should have RT kernel set up correctly and should be able to
# connect to the robot using libfranka.
# For using Franka ROS Interface in this case:
# a) Move this file to the root of the catkin ws, and fill in the
# values for $FRANKA_ROBOT_IP and $your_ip above.
# b) run `./franka.sh master`
# c) Start the driver node (see Usage in README)
# d) Once the driver node is running correctly, on a different
# terminal, run `./franka master` again from the catkin_ws root to
# connect to the same ROS master as the driver node.
# e) You should now be able to communicate with the robot using
# Franka ROS Interface from this terminal session. Use ROS
# topics/services etc. or Franka ROS Interface Python API or scripts
# to control and monitor the robot. (test by running `rostopic list`)
# f) For every new terminal, make sure to run `./franka.sh master` first.
# 2. If you want to use multiple machines for controlling the robot, all
# machines should be in the same network as the robot controller.
# AT LEAST ONE MACHINE should have RT kernel set up and should be able to
# properly connect to the robot using libfranka. ONE of these machines
# should run the driver node as explained in steps 1.a - 1.c above.
# Follow steps 1.d - 1.f if Franka ROS Interface is needed againg on this
# RT machine (which is running the driver).
#
# NOTE: ONLY ONE MACHINE IN THE NETWORK SHOULD BE RUNNING THE DRIVER NODE.
#
# Other computers in the network need not have RT kernels, but should have
# correctly installed lifranka, franka-ros, and Franka ROS Interface. For
# controlling the robot from these machines:
# a) Move this file to the root of the catkin_ws of the remote machines,
# and fill in the values for $FRANKA_ROBOT_IP, $your_ip, and
# $ROS_MASTER_IP above. THE $ROS_MASTER_IP should correspond to the
# IP address of the RT machine running the 'driver' node.
# b) Then run `./franka.sh remote`. This should connect the current
# session to the ROS master running the driver node (to test if the
# connection is correct, run `rostopic list` to see if the published
# and subscribed topics from Franka ROS Interface driver node is
# accessible from the remote computer).
# c) As long as the driver node is running on the RT machine, you should
# now be able to communicate with the robot using Franka ROS Interface
# from this terminal session. Use ROS topics/services etc. or Franka
# ROS Interface Python API or scripts to control and monitor the robot.
# d) For every new terminal in the remote machines, make sure to run
# `./franka.sh remote` first.
## ========================================= #
## ========================================= #
## === DO NOT EDIT BELOW THIS LINE ========= #
if [ "${1}" == "sim" ]; then
your_ip="127.0.0.1"
fi
if [ "${your_ip}" == "" ]; then
echo -e "\n\t\033[01;31m\033[1mError:\033[00m [franka.sh] \033[01;33m\033[4myour_ip\033[00m variable has to be set in 'franka.sh'\n"
exit 1
fi
if [ "${1}" == "master" ] || [[ "${1}" == "local" ]]; then
if [ "${FRANKA_ROBOT_IP}" == "FRANKA_ROBOT_IP.local" ]; then
echo -e "\n\t\033[01;31m\033[1mError:\033[00m [franka.sh] \033[01;33m\033[4mFRANKA_ROBOT_IP\033[00m variable has to be set to match the IP address of the Franka Robot Controller\n"
exit 1
fi
ROS_MASTER_IP="$your_ip"
IS_MASTER=true
elif [ "${1}" == "slave" ] || [[ "${1}" == "remote" ]]; then
if [[ $# -ge 2 ]]; then
ROS_MASTER_IP="$2"
fi
IS_MASTER=false
elif [ "${1}" == "sim" ]; then
ROS_MASTER_IP="$your_ip"
IS_MASTER="true"
FRANKA_ROBOT_IP="sim"
else
echo -e "\n\t\033[01;31m\033[1mError:\033[00m [franka.sh] Atleast one argument has to be passed
Usage: ./franka_sdk <arg1> <arg2>
------ ARGUMENTS:
-- arg1:
- 'master' / 'local' (No <arg2>): The local machine will be the ros master. Run this on the main master pc (connected to the franka controller).
- 'slave' / 'remote': If IP address is provided as <arg2>, this will make sure that this PC is connected to the ROS Master at that IP. If <arg2> provided, ROS_MASTER_IP set below will be used
- 'sim': Franka robot simulation environment will be used. Real robot will not be controlled.
-- arg2:
- <IP of ROS MASTER> (valid only if <arg1> is 'slave'/'remote'): IP address of ROS Master
"
exit 1
fi
tf=$(mktemp)
trap "rm -f -- '${tf}'" EXIT
topdir=$(basename $(readlink -f $(dirname ${BASH_SOURCE[0]})))
cat <<-EOF > ${tf}
[ -s "\${HOME}"/.bashrc ] && source "\${HOME}"/.bashrc
[ -s "\${HOME}"/.bash_profile ] && source "\${HOME}"/.bash_profile
# verify this script is moved out of franka_sdk folder
if [[ -e "${topdir}/franka_rospy_sdk/package.xml" ]]; then
echo -ne "EXITING - This script must be moved from the franka_rospy_sdk folder \
to the root of your catkin workspace.\n"
exit 1
fi
# check for ros installation
if [ ! -d "/opt/ros" ] || [ ! "$(ls -A /opt/ros)" ]; then
echo "EXITING - No ROS installation found in /opt/ros."
echo "Is ROS installed?"
exit 1
fi
# verify specified ros version is installed
ros_setup="/opt/ros/${ros_version}"
if [ ! -d "\${ros_setup}" ]; then
echo -ne "EXITING - Failed to find ROS \${ros_version} installation \
in \${ros_setup}.\n"
exit 1
fi
# verify the ros setup.sh file exists
if [ ! -s "\${ros_setup}"/setup.sh ]; then
echo -ne "EXITING - Failed to find the ROS environment script: \
"\${ros_setup}"/setup.sh.\n"
exit 1
fi
# verify the user is running this script in the root of the catkin
# workspace and that the workspace has been compiled.
if [ ! -s "devel/setup.bash" ]; then
echo -ne "EXITING - \n1) Please verify that this script is being run \
in the root of your catkin workspace.\n2) Please verify that your workspace \
has been built (source /opt/ros/\${ros_version}/setup.sh; catkin_make).\n\
3) Run this script again upon completion of your workspace build.\n"
exit 1
fi
export FRANKA_ROBOT_IP=$FRANKA_ROBOT_IP
[ -n "${your_ip}" ] && export ROS_IP="${your_ip}"
[ -n "${ROS_MASTER_IP}" ] && \
export ROS_MASTER_URI="http://${ROS_MASTER_IP}:11311"
# source the catkin setup bash script
source devel/setup.bash
if [[ $IS_MASTER == true ]]; then
echo -e "\nROBOT: $FRANKA_ROBOT_IP\n"
else
echo -e "\nROS MASTER: $ROS_MASTER_IP\n"
fi
# setup the bash prompt
export __ROS_PROMPT=\${__ROS_PROMPT:-0}
[ \${__ROS_PROMPT} -eq 0 -a -n "\${PROMPT_COMMAND}" ] && \
export __ORIG_PROMPT_COMMAND=\${PROMPT_COMMAND}
__ros_prompt () {
if [ -n "\${__ORIG_PROMPT_COMMAND}" ]; then
eval \${__ORIG_PROMPT_COMMAND}
fi
if ! echo \${PS1} | grep '\[franka' &>/dev/null; then
if [[ $your_ip == "localhost" ]]; then
export PS1="\[\033[00;33m\][franka <SIM> - Robot@\
${FRANKA_ROBOT_IP}]\[\033[00m\] \${PS1}"
elif [[ $IS_MASTER == true ]]; then
export PS1="\[\033[00;33m\][franka <Master> - Robot@\
${FRANKA_ROBOT_IP}]\[\033[00m\] \${PS1}"
else
export PS1="\[\033[00;33m\][franka <Remote> - Master@\
${ROS_MASTER_IP}]\[\033[00m\] \${PS1}"
fi
fi
}
if [ "\${TERM}" != "dumb" ]; then
export PROMPT_COMMAND=__ros_prompt
__ROS_PROMPT=1
elif ! echo \${PS1} | grep '\[franka' &>/dev/null; then
if [[ $your_ip == "localhost" ]]; then
export PS1="\[\033[00;33m\][franka <SIM> - Robot@\
${FRANKA_ROBOT_IP}]\[\033[00m\] \${PS1}"
elif [[ $IS_MASTER == true ]]; then
export PS1="\[\033[00;33m\][franka <Master> - Robot@\
${FRANKA_ROBOT_IP}]\[\033[00m\] \${PS1}"
else
export PS1="\[\033[00;33m\][franka <Remote> - Master@\
${ROS_MASTER_IP}]\[\033[00m\] \${PS1}"
fi
fi
EOF
${SHELL} --rcfile ${tf}
rm -f -- "${tf}"
trap - EXIT
================================================
FILE: franka_common/franka_core_msgs/CMakeLists.txt
================================================
cmake_minimum_required(VERSION 2.8.3)
project(franka_core_msgs)
find_package(catkin REQUIRED COMPONENTS
message_generation
actionlib_msgs
std_msgs
franka_msgs
geometry_msgs
sensor_msgs
control_msgs
)
add_message_files( DIRECTORY msg
FILES
JointCommand.msg
RobotState.msg
EndPointState.msg
JointLimits.msg
JointControllerStates.msg
)
# add_service_files( DIRECTORY srv
# FILES
# SolvePositionIK.srv
# SolvePositionFK.srv
# IOComponentCommandSrv.srv
# )
# add_action_files(DIRECTORY action FILES CalibrationCommand.action)
## Build
generate_messages(DEPENDENCIES std_msgs geometry_msgs control_msgs sensor_msgs actionlib_msgs franka_msgs)
catkin_package(CATKIN_DEPENDS message_runtime std_msgs geometry_msgs control_msgs sensor_msgs franka_msgs actionlib_msgs)
## Install
## Testing
================================================
FILE: franka_common/franka_core_msgs/msg/EndPointState.msg
================================================
std_msgs/Header header
float64[16] O_T_EE # Measured end effector pose in base frame
# ----- Moved ee velocity to robot state, because it is being computed using J*dq. EE vel is not being provided natively
#float64[6] O_dP_EE_c # Last commanded end effector twist in base frame.
#float64[6] O_dP_EE_d # Desired end effector twist in base frame.
#float64[6] O_ddP_EE_c # Last commanded end effector acceleration in base frame.
geometry_msgs/WrenchStamped O_F_ext_hat_K # Estimated external wrench (force, torque) acting on stiffness frame, expressed relative to the base frame
geometry_msgs/WrenchStamped K_F_ext_hat_K # Estimated external wrench (force, torque) acting on stiffness frame, expressed relative to the stiffness frame (ee_frame by default)
================================================
FILE: franka_common/franka_core_msgs/msg/JointCommand.msg
================================================
Header header
int32 mode # Mode in which to command arm
string[] names # Joint names order for command
# Fields of commands indexed according to the Joint names vector.
# Command fields required for a desired mode are listed in the comments
float64[] position # (radians) Required for POSITION_MODE and IMPEDANCE_MODE
float64[] velocity # (radians/sec) Required for VELOCITY_MODE and IMPEDANCE_MODE
float64[] acceleration # (radians/sec^2) Required for
float64[] effort # (newton-meters) Required for TORQUE_MODE
# Modes available to command arm
int32 POSITION_MODE=1
int32 VELOCITY_MODE=2
int32 TORQUE_MODE=3
int32 IMPEDANCE_MODE=4
================================================
FILE: franka_common/franka_core_msgs/msg/JointControllerStates.msg
================================================
Header header
string controller_name
string[] names # Joint names order for command
control_msgs/JointControllerState[] joint_controller_states
================================================
FILE: franka_common/franka_core_msgs/msg/JointLimits.msg
================================================
# names of the joints
string[] joint_names
# lower bound on the angular position in radians
float64[] position_lower
# upper bound on the angular position in radians
float64[] position_upper
# symmetric maximum joint velocity in radians/second
float64[] velocity
# symmetric maximum joint acceleration in radians/second^2
float64[] accel
# symmetric maximum joint torque in Newton-meters
float64[] effort
================================================
FILE: franka_common/franka_core_msgs/msg/RobotState.msg
================================================
std_msgs/Header header
float64[6] cartesian_collision
float64[6] cartesian_contact
float64[6] O_dP_EE # EE vel computed as J*dq
# float64[7] q # joint position, velocity, and effort in joint_states topic
# float64[7] dq
float64[7] q_d
float64[7] dq_d
# float64[7] tau_J
float64[7] dtau_J # torque derivative
float64[7] tau_J_d # desired joint torque
# float64[6] K_F_ext_hat_K
# float64[2] elbow
# float64[2] elbow_d
float64[7] joint_collision
float64[7] joint_contact
# float64[6] O_F_ext_hat_K # in tip state
float64[7] tau_ext_hat_filtered # filtered external torque
float64[3] F_x_Cee # Configured center of mass of the end effector load with respect to flange frame.
float64[3] F_x_Cload # Configured center of mass of the external load with respect to flange frame.
float64[3] F_x_Ctotal # Combined center of mass of the end effector load and the external load with respect to flange frame.
float64[9] I_ee # Configured rotational inertia matrix of the end effector load with respect to center of mass.
float64[9] I_load
float64[9] I_total
float64 m_ee # configured mass of end-effector
float64 m_load # configured mass of external load
float64 m_total
float64[7] gravity
float64[7] coriolis
float64[49] mass_matrix # mass matrix of end-effector wrt base frame # Vectorized 7x7, column-major
float64[42] O_Jac_EE # zero jacobian of end-effector frame. Vectorized 6x7 Jacobian, column-major
# float64[16] O_T_EE # ----- moved to endpointstate msg # Vectorized 4x4, column-major
float64[16] O_T_EE_d # Last desired end effector pose of motion generation in base frame. # Vectorized 4x4, column-major
float64[16] F_T_EE # End effector frame pose in flange frame # Vectorized 4x4, column-major
float64[16] F_T_NE # Nominal End effector frame pose in flange frame (fixed in Desk) # Vectorized 4x4, column-major
float64[16] NE_T_EE # End effector frame pose in Nominal End effector frame # Vectorized 4x4, column-major
float64[16] EE_T_K # Stiffness frame pose in end effector frame # Vectorized 4x4, column-major
float64 time
uint8 ROBOT_MODE_OTHER=0
uint8 ROBOT_MODE_IDLE=1
uint8 ROBOT_MODE_MOVE=2
uint8 ROBOT_MODE_GUIDING=3
uint8 ROBOT_MODE_REFLEX=4
uint8 ROBOT_MODE_USER_STOPPED=5
uint8 ROBOT_MODE_AUTOMATIC_ERROR_RECOVERY=6
uint8 robot_mode
franka_msgs/Errors current_errors
franka_msgs/Errors last_motion_errors
================================================
FILE: franka_common/franka_core_msgs/package.xml
================================================
<?xml version="1.0"?>
<package>
<name>franka_core_msgs</name>
<version>0.7.1</version>
<description>
Messages and Services required for communication with the Franka robot when using
franka_ros_interface and panda_simulator
</description>
<maintainer email="sxs1412@bham.ac.uk">
Saif Sidhik
</maintainer>
<license>Apache 2.0</license>
<author>Saif Sidhik</author>
<buildtool_depend>catkin</buildtool_depend>
<!-- package contains messages -->
<build_depend>message_generation</build_depend>
<build_depend>std_msgs</build_depend>
<build_depend>geometry_msgs</build_depend>
<build_depend>sensor_msgs</build_depend>
<build_depend>control_msgs</build_depend>
<build_depend>franka_msgs</build_depend>
<build_depend>actionlib_msgs</build_depend>
<run_depend>message_runtime</run_depend>
<run_depend>std_msgs</run_depend>
<run_depend>franka_msgs</run_depend>
<run_depend>geometry_msgs</run_depend>
<run_depend>control_msgs</run_depend>
<run_depend>sensor_msgs</run_depend>
<run_depend>actionlib_msgs</run_depend>
</package>
================================================
FILE: franka_interface/CMakeLists.txt
================================================
cmake_minimum_required(VERSION 2.8.3)
project(franka_interface)
set(CMAKE_BUILD_TYPE Release)
set(CMAKE_CXX_STANDARD 14)
set(CMAKE_CXX_STANDARD_REQUIRED ON)
find_package(catkin REQUIRED COMPONENTS
controller_interface
dynamic_reconfigure
franka_hw
franka_control
controller_manager
franka_core_msgs
geometry_msgs
hardware_interface
pluginlib
realtime_tools
roscpp
rospy
)
find_package(Eigen3 REQUIRED)
find_package(Franka 0.5.0 REQUIRED)
catkin_package(
LIBRARIES custom_franka_state_controller
CATKIN_DEPENDS
controller_interface
franka_msgs
dynamic_reconfigure
franka_hw
franka_control
franka_core_msgs
geometry_msgs
hardware_interface
pluginlib
realtime_tools
roscpp
DEPENDS Franka
)
add_library(custom_franka_state_controller
src/robot_state_controller.cpp
)
add_dependencies(custom_franka_state_controller
${${PROJECT_NAME}_EXPORTED_TARGETS}
${catkin_EXPORTED_TARGETS}
)
target_link_libraries(custom_franka_state_controller PUBLIC
${Franka_LIBRARIES}
${catkin_LIBRARIES}
)
target_include_directories(custom_franka_state_controller SYSTEM PUBLIC
${Franka_INCLUDE_DIRS}
${EIGEN3_INCLUDE_DIRS}
${catkin_INCLUDE_DIRS}
)
target_include_directories(custom_franka_state_controller PUBLIC
include
)
## franka_control_node
add_executable(custom_franka_control_node
src/franka_control_node.cpp
src/motion_controller_interface.cpp
)
add_dependencies(custom_franka_control_node
${${PROJECT_NAME}_EXPORTED_TARGETS}
${catkin_EXPORTED_TARGETS}
)
target_link_libraries(custom_franka_control_node
${Franka_LIBRARIES}
${franka_control_LIBRARIES}
# franka_control_services
${catkin_LIBRARIES}
)
target_include_directories(custom_franka_control_node SYSTEM PUBLIC
${Franka_INCLUDE_DIRS}
${catkin_INCLUDE_DIRS}
include
)
## Installation
install(TARGETS custom_franka_state_controller
custom_franka_control_node
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
install(DIRECTORY launch
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)
install(DIRECTORY config
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)
install(FILES state_controller_plugin.xml
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)
include(${CMAKE_CURRENT_LIST_DIR}/../cmake/ClangTools.cmake OPTIONAL
RESULT_VARIABLE CLANG_TOOLS
)
if(CLANG_TOOLS)
file(GLOB_RECURSE SOURCES ${CMAKE_CURRENT_SOURCE_DIR}/src/*.cpp)
file(GLOB_RECURSE HEADERS
${CMAKE_CURRENT_SOURCE_DIR}/include/*.h
${CMAKE_CURRENT_SOURCE_DIR}/src/*.h
)
add_format_target(custom_franka_state_controller FILES ${SOURCES} ${HEADERS})
add_tidy_target(custom_franka_state_controller custom_franka_control_node
FILES ${SOURCES}
DEPENDS custom_franka_state_controller
)
endif()
catkin_python_setup()
catkin_install_python(PROGRAMS scripts/enable_robot.py
scripts/move_to_neutral.py
scripts/simple_gripper.py
tests/demo_joint_positions_keyboard.py
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
# ## Tools
# include(${CMAKE_CURRENT_LIST_DIR}/../cmake/ClangTools.cmake OPTIONAL
# RESULT_VARIABLE CLANG_TOOLS
# )
# if(CLANG_TOOLS)
# file(GLOB_RECURSE SOURCES ${CMAKE_CURRENT_SOURCE_DIR}/src/*.cpp)
# file(GLOB_RECURSE HEADERS
# ${CMAKE_CURRENT_SOURCE_DIR}/include/*.h
# ${CMAKE_CURRENT_SOURCE_DIR}/src/*.h
# )
# add_format_target(custom_franka_state_controller FILES ${SOURCES} ${HEADERS})
# add_tidy_target(custom_franka_state_controller
# FILES ${SOURCES}
# DEPENDS custom_franka_state_controller
# )
# endif()
================================================
FILE: franka_interface/README.md
================================================
# franka_interface
**See induvidual source files for more detailed documentation.**
### ArmInterface
- Interface class that can monitor and control the robot
- Provides all required information about robot state and end-effector state
- Joint positions, velocities, and effort can be directly controlled and monitored using available methods
- Smooth interpolation of joint positions possible
- End-effector and Stiffness frames can be directly set (uses FrankaFramesInterface from *franka_ros_interface/franka_tools*)
### GripperInterface
- Interface class to monitor and control gripper
- Gripper open, close methods
- Grasp, move joints methods
### RobotEnable
- Interface class to reset robot when in recoverable error (use *enable_robot.py* script in *scripts/*)
### RobotParams
- Collects and stores all useful information about the robot from the ROS parameter server
### Scripts
- Scripts providing simple gripper actions, and robot reset are provided
================================================
FILE: franka_interface/config/basic_controllers.yaml
================================================
# load the internal joint trajectory controller, and the custom robot state controller
position_joint_trajectory_controller:
type: position_controllers/JointTrajectoryController
joints:
- panda_joint1
- panda_joint2
- panda_joint3
- panda_joint4
- panda_joint5
- panda_joint6
- panda_joint7
constraints:
goal_time: 0.5
panda_joint1:
goal: 0.05
panda_joint2:
goal: 0.05
panda_joint3:
goal: 0.05
panda_joint4:
goal: 0.05
panda_joint5:
goal: 0.05
panda_joint6:
goal: 0.05
panda_joint7:
goal: 0.05
franka_ros_interface:
custom_franka_state_controller:
type: franka_interface/CustomFrankaStateController
publish_rate: 1000 # [Hz]
joint_names:
- panda_joint1
- panda_joint2
- panda_joint3
- panda_joint4
- panda_joint5
- panda_joint6
- panda_joint7
arm_id: panda
================================================
FILE: franka_interface/config/robot_config.yaml
================================================
# global robot configuration and details
robot_config:
joint_names:
- panda_joint1
- panda_joint2
- panda_joint3
- panda_joint4
- panda_joint5
- panda_joint6
- panda_joint7
arm_id: panda
# Activate rate limiter? [true|false]
joint_limit_warning_threshold: 0.1 # [rad]
rate_limiting: true
# Cutoff frequency of the low-pass filter. Set to >= 1000 to deactivate.
cutoff_frequency: 100
# Internal controller for motion generators [joint_impedance|cartesian_impedance]
internal_controller: joint_impedance
neutral_pose:
panda_joint1: -0.017792060227770554
panda_joint2: -0.7601235411041661
panda_joint3: 0.019782607023391807
panda_joint4: -2.342050140544315
panda_joint5: 0.029840531355804868
panda_joint6: 1.5411935298621688
panda_joint7: 0.7534486589746342
joint_config:
joint_acceleration_limit:
panda_joint1: 15.0 # rad / sec^2
panda_joint2: 7.5 # rad / sec^2
panda_joint3: 10.0 # rad / sec^2
panda_joint4: 12.5 # rad / sec^2
panda_joint5: 15.0 # rad / sec^2
panda_joint6: 20.0 # rad / sec^2
panda_joint7: 20.0 # rad / sec^2
# panda_finger_joint1: 12.0 # rad / sec^2
# panda_finger_joint2: 12.0 # rad / sec^2
joint_velocity_limit:
panda_joint1: 2.1750 # rad / sec
panda_joint2: 2.1750 # rad / sec
panda_joint3: 2.1750 # rad / sec
panda_joint4: 2.1750 # rad / sec
panda_joint5: 2.6100 # rad / sec
panda_joint6: 2.6100 # rad / sec
panda_joint7: 2.6100 # rad / sec
# panda_finger_joint1: 2.0 # rad / sec
# panda_finger_joint2: 2.0 # rad / sec
joint_position_limit:
lower:
panda_joint1: -2.8973 # rad
panda_joint2: -1.7628 # rad
panda_joint3: -2.8973 # rad
panda_joint4: -3.0718 # rad
panda_joint5: -2.8973 # rad
panda_joint6: -0.0175 # rad
panda_joint7: -2.8973 # rad
upper:
panda_joint1: 2.8973 # rad
panda_joint2: 1.7628 # rad
panda_joint3: 2.8973 # rad
panda_joint4: -0.0698 # rad
panda_joint5: 2.8973 # rad
panda_joint6: 3.7525 # rad
panda_joint7: 2.8973 # rad
joint_effort_limit:
panda_joint1: 87 # Nm
panda_joint2: 87 # Nm
panda_joint3: 87 # Nm
panda_joint4: 87 # Nm
panda_joint5: 12 # Nm
panda_joint6: 12 # Nm
panda_joint7: 12 # Nm
realtime_config: enforce
# Configure the initial defaults for the collision behavior reflexes.
collision_config:
lower_torque_thresholds_acceleration: [20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0] # [Nm]
upper_torque_thresholds_acceleration: [20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0] # [Nm]
lower_torque_thresholds_nominal: [20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0] # [Nm]
upper_torque_thresholds_nominal: [20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0] # [Nm]
lower_force_thresholds_acceleration: [20.0, 20.0, 20.0, 25.0, 25.0, 25.0] # [N, N, N, Nm, Nm, Nm]
upper_force_thresholds_acceleration: [20.0, 20.0, 20.0, 25.0, 25.0, 25.0] # [N, N, N, Nm, Nm, Nm]
lower_force_thresholds_nominal: [20.0, 20.0, 20.0, 25.0, 25.0, 25.0] # [N, N, N, Nm, Nm, Nm]
upper_force_thresholds_nominal: [20.0, 20.0, 20.0, 25.0, 25.0, 25.0] # [N, N, N, Nm, Nm, Nm]
gripper_config:
joint_names:
- panda_finger_joint1
- panda_finger_joint2
default_speed: 0.1 # [m/s]
default_grasp_epsilon:
inner: 0.005 # [m]
outer: 0.005 # [m]
controllers_config:
position_controller: "franka_ros_interface/position_joint_position_controller"
torque_controller: "franka_ros_interface/effort_joint_torque_controller"
impedance_controller: "franka_ros_interface/effort_joint_impedance_controller"
velocity_controller: "franka_ros_interface/velocity_joint_velocity_controller"
trajectory_controller: "position_joint_trajectory_controller"
default_controller: "position_joint_trajectory_controller" # for safety, always set a position controller as default
command_timeout: 0.2 # timeout to wait for consecutive torque control commands (or velocity) when using torque (or velocity) control. If timeout is violated, the controller interface will automatically switch to default controller for safety
================================================
FILE: franka_interface/include/franka_interface/motion_controller_interface.h
================================================
/***************************************************************************
* Adapted from sawyer_simulator package
*
* @package: franka_interface
* @metapackage: franka_ros_interface
* @author: Saif Sidhik <sxs1412@bham.ac.uk>
*
**************************************************************************/
/***************************************************************************
* Copyright (c) 2019-2021, Saif Sidhik.
* Copyright (c) 2013-2018, Rethink Robotics Inc.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
**************************************************************************/
#ifndef _FRANKA_INTERFACE____MOTION_CONTROLLER_INTERFACE_H_
#define _FRANKA_INTERFACE____MOTION_CONTROLLER_INTERFACE_H_
#include <mutex>
#include <ros/ros.h>
#include <controller_manager/controller_manager.h>
#include <std_msgs/Float64.h>
#include <realtime_tools/realtime_box.h>
#include <franka_core_msgs/JointCommand.h>
namespace franka_interface {
class MotionControllerInterface
{
public:
/**
* Initializes the controller manager.
*
* @param[in] root_node_handle Node handle in the controller_manager namespace.
* @param[in] controller_manager the controller manager instance.
*/
void init(ros::NodeHandle& nh,
boost::shared_ptr<controller_manager::ControllerManager> controller_manager);
private:
// mutex for re-entrant calls to modeCommandCallback
std::mutex mtx_;
int current_mode_;
realtime_tools::RealtimeBox< std::shared_ptr<const ros::Duration > > box_timeout_length_;
realtime_tools::RealtimeBox< std::shared_ptr<const ros::Time > > box_cmd_timeout_;
ros::Timer cmd_timeout_timer_;
ros::Subscriber joint_command_timeout_sub_;
ros::Subscriber joint_command_sub_;
boost::shared_ptr<controller_manager::ControllerManager> controller_manager_;
std::string position_controller_name_;
std::string velocity_controller_name_;
std::string torque_controller_name_;
std::string impedance_controller_name_;
std::string trajectory_controller_name_;
std::string default_controller_name_;
std::string current_controller_name_;
std::vector<std::string> all_controllers_;
std::map<std::string,int> controller_name_to_mode_map_;
protected:
/**
* Callback function to set time out to switch back to position mode. When using torque
* mode controllers, consecutive torque commands should be sent in smaller intervals than
* this timeout value
*
* @param[in] msg Float64 instance containing the desired timeout.
*/
void jointCommandTimeoutCallback(const std_msgs::Float64 msg);
/**
* Callback function to choose the appropriate controller. This function checks the type
* of controller requested by the client through the type field in the message, and
* starts the controller.
*
* @param[in] msg JointCommandConstPtr instance containing the joint commands.
*/
void jointCommandCallback(const franka_core_msgs::JointCommandConstPtr& msg);
/**
* Switch controller depending on the mode specified.
*
* @param[in] control_mode integer value representing the type of controller to use
*/
bool switchControllers(int control_mode);
/**
* Switch controller to the initially defined default controller.
*/
bool switchToDefaultController();
/**
* Check if the command timeout has been violated.
*
* @param[in] control_mode integer value representing the type of controller to use
*/
void commandTimeoutCheck(const ros::TimerEvent& e);
};
}
#endif // #ifndef _FRANKA_INTERFACE____MOTION_CONTROLLER_INTERFACE_H_
================================================
FILE: franka_interface/include/franka_interface/robot_state_controller.h
================================================
/***************************************************************************
* Adapted from robot_state_controller.cpp (franka_ros package)
*
* @package: franka_interface
* @metapackage: franka_ros_interface
* @author: Saif Sidhik <sxs1412@bham.ac.uk>
*
**************************************************************************/
/***************************************************************************
* Copyright (c) 2019-2021, Saif Sidhik
* Copyright (c) 2017 Franka Emika GmbH
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
**************************************************************************/
#pragma once
#include <cstdint>
#include <memory>
#include <vector>
#include <controller_interface/multi_interface_controller.h>
#include <franka_hw/franka_state_interface.h>
#include <franka_hw/franka_model_interface.h>
#include <franka_hw/trigger_rate.h>
#include <franka_core_msgs/RobotState.h>
#include <franka_core_msgs/EndPointState.h>
#include <geometry_msgs/WrenchStamped.h>
#include <realtime_tools/realtime_publisher.h>
#include <sensor_msgs/JointState.h>
#include <tf2_msgs/TFMessage.h>
#include <controller_manager/controller_manager.h>
#include <Eigen/Dense>
namespace franka_interface {
/**
* Custom controller to publish the robot state as ROS topics.
*/
class CustomFrankaStateController
: public controller_interface::MultiInterfaceController<franka_hw::FrankaStateInterface,
franka_hw::FrankaModelInterface> {
public:
/**
* Creates an instance of a CustomFrankaStateController.
*/
CustomFrankaStateController() = default;
/**
* Initializes the controller with interfaces and publishers.
*
* @param[in] robot_hardware RobotHW instance to get a franka_hw::FrankaStateInterface from.
* @param[in] root_node_handle Node handle in the controller_manager namespace.
* @param[in] controller_node_handle Node handle in the controller namespace.
*/
bool init(hardware_interface::RobotHW* robot_hardware,
ros::NodeHandle& root_node_handle,
ros::NodeHandle& controller_node_handle) override;
/**
* Reads the current robot state from the franka_hw::FrankaStateInterface and publishes it.
*
* @param[in] time Current ROS time.
* @param[in] period Time since the last update.
*/
void update(const ros::Time& time, const ros::Duration& period) override;
private:
void publishFrankaState(const ros::Time& time);
void publishJointStates(const ros::Time& time);
void publishTransforms(const ros::Time& time);
void publishEndPointState(const ros::Time& time);
std::string arm_id_;
franka_hw::FrankaStateInterface* franka_state_interface_{};
std::unique_ptr<franka_hw::FrankaStateHandle> franka_state_handle_{};
std::unique_ptr<franka_hw::FrankaModelHandle> model_handle_;
realtime_tools::RealtimePublisher<tf2_msgs::TFMessage> publisher_transforms_;
realtime_tools::RealtimePublisher<franka_core_msgs::RobotState> publisher_franka_state_;
realtime_tools::RealtimePublisher<sensor_msgs::JointState> publisher_joint_states_;
realtime_tools::RealtimePublisher<sensor_msgs::JointState> publisher_joint_states_desired_;
realtime_tools::RealtimePublisher<franka_core_msgs::EndPointState> publisher_tip_state_;
franka_hw::TriggerRate trigger_publish_;
franka::RobotState robot_state_;
uint64_t sequence_number_ = 0;
std::vector<std::string> joint_names_;
};
} // namespace franka_interface
================================================
FILE: franka_interface/launch/interface.launch
================================================
<?xml version="1.0" ?>
<launch>
<rosparam command="load" file="$(find franka_ros_controllers)/config/ros_controllers.yaml" ns="/franka_ros_interface"/>
<arg name="load_gripper" default="true" />
<arg name="rate" default="1000" />
<arg name="start_controllers" default="true" />
<arg name="start_moveit" default="true" />
<arg name="fake_execution" default="false" /> <!-- Only valid if running moveit movegroup (start_moveit:=true) -->
<arg name="load_demo_planning_scene" default="true"/>
<!-- Panda Control Interface -->
<param name="robot_description" command="$(find xacro)/xacro --inorder '$(find franka_panda_description)/robots/panda_arm_hand.urdf.xacro'" if="$(arg load_gripper)" />
<param name="robot_description" command="$(find xacro)/xacro --inorder '$(find franka_panda_description)/robots/panda_arm.urdf.xacro'" unless="$(arg load_gripper)" />
<include file="$(find franka_gripper)/launch/franka_gripper.launch" if="$(arg load_gripper)">
<arg name="robot_ip" value="$(env FRANKA_ROBOT_IP)" />
</include>
<!-- Start the custom_franka_control_node for advertising controller services and starting custom controller manager-->
<rosparam command="load" file="$(find franka_interface)/config/robot_config.yaml"/>
<param name="/robot_config/robot_ip" type="str" value="$(env FRANKA_ROBOT_IP)" />
<node name="franka_control" pkg="franka_interface" type="custom_franka_control_node" output="screen" required="true" >
<!-- <rosparam command="load" file="$(find franka_control)/config/custom_franka_control_node.yaml" /> -->
<!-- <param name="robot_ip" value="$(env FRANKA_ROBOT_IP)" /> -->
<param name="publish_frequency" value="$(arg rate)"/>
</node>
<!-- Start the custom state publisher for franka_ros_interface -->
<rosparam command="load" file="$(find franka_interface)/config/basic_controllers.yaml"/>
<node name="state_controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="franka_ros_interface/custom_franka_state_controller" />
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" output="screen">
<param name="publish_frequency" value="$(arg rate)"/>
</node>
<!-- Start joint_state_publisher with the joint states of the robot -->
<node name="joint_state_publisher" type="joint_state_publisher" pkg="joint_state_publisher" output="screen">
<rosparam if="$(arg load_gripper)" param="source_list">[franka_ros_interface/custom_franka_state_controller/joint_states, franka_gripper/joint_states] </rosparam>
<rosparam unless="$(arg load_gripper)" param="source_list">[franka_ros_interface/custom_franka_state_controller/joint_states] </rosparam>
<param name="rate" value="$(arg rate)"/>
</node>
<node name="joint_state_desired_publisher" type="joint_state_publisher" pkg="joint_state_publisher" output="screen">
<rosparam if="$(arg load_gripper)" param="source_list">[franka_ros_interface/custom_franka_state_controller/joint_states_desired, franka_gripper/joint_states] </rosparam>
<rosparam unless="$(arg load_gripper)" param="source_list">[franka_ros_interface/custom_franka_state_controller/joint_states_desired] </rosparam>
<param name="rate" value="$(arg rate)"/>
<remap from="/joint_states" to="/joint_states_desired" />
</node>
<node name="controllers" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="position_joint_trajectory_controller"/>
<!-- Load the custom controllers -->
<group if="$(eval arg('start_controllers') == true)">
<node name="load_controllers" pkg="controller_manager" type="controller_manager" respawn="false"
output="screen" args="load
franka_ros_interface/effort_joint_impedance_controller
franka_ros_interface/effort_joint_position_controller
franka_ros_interface/effort_joint_torque_controller
franka_ros_interface/velocity_joint_velocity_controller
franka_ros_interface/position_joint_position_controller"/>
</group>
<node pkg="tf" type="static_transform_publisher" name="base_to_link0" args="0 0 0 0 0 0 1 base panda_link0 100" />
<node pkg="tf" type="static_transform_publisher" name="world_to_base" args="0 0 0 0 0 0 1 world base 100" />
<group if="$(eval arg('start_moveit') == true)">
<include file="$(find panda_moveit_config)/launch/move_group.launch">
<arg name="fake_execution" value="$(arg fake_execution)" />
<arg name="load_gripper" value="$(arg load_gripper)" />
<arg name="info" value="true" />
</include>
<group if="$(eval arg('load_demo_planning_scene') == true)">
<node name="demo_scene_loader" pkg="franka_moveit" type="create_demo_planning_scene.py" respawn="false" output="screen" />
</group>
</group>
</launch>
================================================
FILE: franka_interface/package.xml
================================================
<?xml version="1.0"?>
<package format="2">>
<name>franka_interface</name>
<version>0.7.1</version>
<description>
ROS/Python interface classes for control of
Franka Panda robot through franka-ros and libfranka.
</description>
<maintainer email="sxs1412@bham.ac.uk">
Saif Sidhik
</maintainer>
<license>Apache 2.0</license>
<author>Saif Sidhik</author>
<buildtool_depend>catkin</buildtool_depend>
<build_depend>message_generation</build_depend>
<build_depend>eigen</build_depend>
<build_depend>franka_control</build_depend>
<depend>controller_interface</depend>
<depend>dynamic_reconfigure</depend>
<depend>franka_hw</depend>
<depend>controller_manager</depend>
<depend>franka_core_msgs</depend>
<depend>franka_msgs</depend>
<depend>geometry_msgs</depend>
<depend>hardware_interface</depend>
<depend>libfranka</depend>
<depend>pluginlib</depend>
<depend>realtime_tools</depend>
<depend>roscpp</depend>
<exec_depend>franka_control</exec_depend>
<exec_depend>franka_description</exec_depend>
<exec_depend>panda_moveit_config</exec_depend>
<exec_depend>franka_moveit</exec_depend>
<exec_depend>rospy</exec_depend>
<export>
<controller_interface plugin="${prefix}/state_controller_plugin.xml"/>
</export>
</package>
================================================
FILE: franka_interface/scripts/enable_robot.py
================================================
#!/usr/bin/env python
# /***************************************************************************
#
# @package: franka_interface
# @metapackage: franka_ros_interface
# @author: Saif Sidhik <sxs1412@bham.ac.uk>
#
# **************************************************************************/
# /***************************************************************************
# Copyright (c) 2019-2021, Saif Sidhik
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
# http://www.apache.org/licenses/LICENSE-2.0
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
# **************************************************************************/
"""
:info:
Utility script for enabling, disabling and checking status of robot.
Usage:
enable_robot.py <arg>
:Args:
<arg> : -s (--state) / -e (--enable) / -d (--disable) / -r (--reset) / -S (--stop)
Not implemented: Disabling robot.
Todo: Change reset request to service request instead of publishing message to topic.
"""
import argparse
import sys
import rospy
import franka_interface
def main():
rospy.init_node('panda_robot_enable')
parser = argparse.ArgumentParser()
parser.add_argument('-s', '--state', const='state',
dest='actions', action='append_const',
help='Print current robot state')
parser.add_argument('-e', '--enable', const='enable',
dest='actions', action='append_const',
help='Enable the robot')
parser.add_argument('-d', '--disable', const='disable',
dest='actions', action='append_const',
help='Disable the robot')
parser.add_argument('-r', '--reset', const='reset',
dest='actions', action='append_const',
help='Reset the robot')
parser.add_argument('-S', '--stop', const='stop',
dest='actions', action='append_const',
help='Stop the robot')
args = parser.parse_args(rospy.myargv()[1:])
if args.actions == None:
parser.print_usage()
parser.exit(0, "No action defined\n")
rs = franka_interface.RobotEnable()
try:
for act in args.actions:
if act == 'state':
print(rs.state())
elif act == 'enable':
rs.enable()
elif act == 'disable':
rs.disable()
elif act == 'reset':
rs.reset()
elif act == 'stop':
rs.stop()
except Exception as e:
rospy.logerr(e)
return 0
if __name__ == '__main__':
sys.exit(main())
================================================
FILE: franka_interface/scripts/move_to_neutral.py
================================================
#!/usr/bin/env python
# /***************************************************************************
#
# @package: franka_interface
# @metapackage: franka_ros_interface
# @author: Saif Sidhik <sxs1412@bham.ac.uk>
#
# **************************************************************************/
# /***************************************************************************
# Copyright (c) 2019-2021, Saif Sidhik
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
# http://www.apache.org/licenses/LICENSE-2.0
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
# **************************************************************************/
"""
:info:
commands robot to move to neutral pose
"""
import rospy
from franka_interface import ArmInterface
if __name__ == '__main__':
rospy.init_node("move_to_neutral_node")
r = ArmInterface()
r.move_to_neutral()
================================================
FILE: franka_interface/scripts/simple_gripper.py
================================================
#!/usr/bin/env python
# /***************************************************************************
#
# @package: franka_interface
# @metapackage: franka_ros_interface
# @author: Saif Sidhik <sxs1412@bham.ac.uk>
#
# **************************************************************************/
# /***************************************************************************
# Copyright (c) 2019-2021, Saif Sidhik
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
# http://www.apache.org/licenses/LICENSE-2.0
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
# **************************************************************************/
"""
:info:
NOTE: Running this script without any arguments will stop the current command being
sent to the gripper. Use this to stop any dangerous action being run
Utility script for simple gripper motion.
Usage:
python simple_gripper.py <arg>
@Args:
<No arg> : Stop current gripper action
-o (--open) : optional argument to make the gripper release and go to max width
-c (--close) : optional argument to close the gripper
"""
import rospy
import argparse
import actionlib
from franka_interface import GripperInterface
from franka_gripper.msg import ( StopAction, StopGoal )
def _active_cb():
rospy.loginfo("GripperInterface: '{}' request active.".format(_caller))
def _feedback_cb(msg):
rospy.loginfo("GripperInterface: '{}' request feedback: \n\t{}".format(_caller,msg))
def _done_cb(status, result):
rospy.loginfo("GripperInterface: '{}' complete. Result: \n\t{}".format(_caller, result))
if __name__ == '__main__':
rospy.init_node("gripper_stop_node")
_caller = "stop_gripper"
stop_action_client = actionlib.SimpleActionClient("/franka_gripper/stop", StopAction)
stop_action_client.wait_for_server()
stop_action_client.send_goal(StopGoal(), done_cb =_done_cb, active_cb = _active_cb, feedback_cb = _feedback_cb)
result = stop_action_client.wait_for_result(rospy.Duration(15.))
parser = argparse.ArgumentParser("Stop current gripper action; Open or close gripper.")
gripper_group = parser.add_mutually_exclusive_group(required=False)
gripper_group.add_argument("-o", "--open", dest="open",
action='store_true', default=False, help="open gripper")
gripper_group.add_argument("-c", "--close", dest="close",
action='store_true', default=False, help="close gripper")
args = parser.parse_args(rospy.myargv()[1:])
if args.open:
gi = GripperInterface()
gi.open()
else:
gi = GripperInterface()
gi.close()
================================================
FILE: franka_interface/setup.py
================================================
#!/usr/bin/env python
from distutils.core import setup
from catkin_pkg.python_setup import generate_distutils_setup
d = generate_distutils_setup()
d['packages'] = ['franka_interface','franka_dataflow']
d['package_dir'] = {'': 'src'}
setup(**d)
================================================
FILE: franka_interface/src/franka_control_node.cpp
================================================
// /***************************************************************************
// * Adapted from franka_control_node.cpp (frank_ros package)
// *
// * @package: franka_interface
// * @metapackage: franka_ros_interface
// * @author: Saif Sidhik <sxs1412@bham.ac.uk>
// *
// **************************************************************************/
// /***************************************************************************
// * Copyright (c) 2019-2021, Saif Sidhik.
// * Copyright (c) 2017 Franka Emika GmbH
// *
// * Licensed under the Apache License, Version 2.0 (the "License");
// * you may not use this file except in compliance with the License.
// * You may obtain a copy of the License at
// *
// * http://www.apache.org/licenses/LICENSE-2.0
// *
// * Unless required by applicable law or agreed to in writing, software
// * distributed under the License is distributed on an "AS IS" BASIS,
// * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// * See the License for the specific language governing permissions and
// * limitations under the License.
// **************************************************************************/
#include <algorithm>
#include <atomic>
#include <actionlib/server/simple_action_server.h>
#include <controller_manager/controller_manager.h>
#include <franka/exception.h>
#include <franka/robot.h>
#include <franka_hw/franka_hw.h>
#include <franka_hw/services.h>
#include <franka_msgs/ErrorRecoveryAction.h>
#include <ros/ros.h>
#include <franka_interface/motion_controller_interface.h>
using franka_hw::ServiceContainer;
int main(int argc, char** argv) {
ros::init(argc, argv, "franka_control_node");
ros::NodeHandle public_node_handle;
ros::NodeHandle config_node_handle("/robot_config");
ros::NodeHandle base_node_handle("~");
franka_hw::FrankaHW franka_control;
if (!franka_control.init(public_node_handle, config_node_handle)) {
ROS_ERROR("franka_control_node: Failed to initialize FrankaHW class. Shutting down!");
return 1;
}
franka::Robot& robot = franka_control.robot();
std::atomic_bool has_error(false);
ServiceContainer services;
services
.advertiseService<franka_msgs::SetJointImpedance>(base_node_handle, "/franka_ros_interface/franka_control/set_joint_impedance",
[&robot](auto&& req, auto&& res) {
return franka_hw::setJointImpedance(
robot, req, res);
})
.advertiseService<franka_msgs::SetCartesianImpedance>(
base_node_handle, "/franka_ros_interface/franka_control/set_cartesian_impedance",
[&robot](auto&& req, auto&& res) {
return franka_hw::setCartesianImpedance(robot, req, res);
})
.advertiseService<franka_msgs::SetEEFrame>(
base_node_handle, "/franka_ros_interface/franka_control/set_EE_frame",
[&robot](auto&& req, auto&& res) { return franka_hw::setEEFrame(robot, req, res); })
.advertiseService<franka_msgs::SetKFrame>(
base_node_handle, "/franka_ros_interface/franka_control/set_K_frame",
[&robot](auto&& req, auto&& res) { return franka_hw::setKFrame(robot, req, res); })
.advertiseService<franka_msgs::SetForceTorqueCollisionBehavior>(
base_node_handle, "/franka_ros_interface/franka_control/set_force_torque_collision_behavior",
[&robot](auto&& req, auto&& res) {
return franka_hw::setForceTorqueCollisionBehavior(robot, req, res);
})
.advertiseService<franka_msgs::SetFullCollisionBehavior>(
base_node_handle, "/franka_ros_interface/franka_control/set_full_collision_behavior",
[&robot](auto&& req, auto&& res) {
return franka_hw::setFullCollisionBehavior(robot, req, res);
})
.advertiseService<franka_msgs::SetLoad>(
base_node_handle, "/franka_ros_interface/franka_control/set_load",
[&robot](auto&& req, auto&& res) { return franka_hw::setLoad(robot, req, res); });
actionlib::SimpleActionServer<franka_msgs::ErrorRecoveryAction> recovery_action_server(
base_node_handle, "/franka_ros_interface/franka_control/error_recovery",
[&](const franka_msgs::ErrorRecoveryGoalConstPtr&) {
try {
robot.automaticErrorRecovery();
has_error = false;
recovery_action_server.setSucceeded();
ROS_INFO("Recovered from error");
} catch (const franka::Exception& ex) {
recovery_action_server.setAborted(franka_msgs::ErrorRecoveryResult(), ex.what());
}
},
false);
franka_control.update(robot.readOnce());
boost::shared_ptr<controller_manager::ControllerManager> control_manager;
control_manager.reset(new controller_manager::ControllerManager(&franka_control, public_node_handle));
franka_interface::MotionControllerInterface motion_controller_interface_;
motion_controller_interface_.init(public_node_handle, control_manager);
recovery_action_server.start();
// // Start background threads for message handling
ros::AsyncSpinner spinner(4);
spinner.start();
while (ros::ok()) {
ros::Time last_time = ros::Time::now();
// Wait until controller has been activated or error has been recovered
while (!franka_control.controllerActive() || has_error) {
franka_control.update(robot.readOnce());
ros::Time now = ros::Time::now();
control_manager->update(now, now - last_time);
franka_control.checkJointLimits();
last_time = now;
if (!ros::ok()) {
return 0;
}
}
try {
// Run control loop. Will exit if the controller is switched.
franka_control.control([&](const ros::Time& now, const ros::Duration& period) {
if (period.toSec() == 0.0) {
// Reset controllers before starting a motion
control_manager->update(now, period, true);
franka_control.checkJointLimits();
franka_control.reset();
} else {
control_manager->update(now, period);
franka_control.checkJointLimits();
franka_control.enforceLimits(period);
}
return ros::ok();
});
} catch (const franka::ControlException& e) {
ROS_ERROR("%s", e.what());
has_error = true;
}
}
return 0;
}
================================================
FILE: franka_interface/src/franka_dataflow/__init__.py
================================================
# /***************************************************************************
# Copyright (c) 2019-2021, Saif Sidhik
# Copyright (c) 2013-2018, Rethink Robotics Inc.
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
# http://www.apache.org/licenses/LICENSE-2.0
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
# **************************************************************************/
from .wait_for import wait_for
================================================
FILE: franka_interface/src/franka_dataflow/getch.py
================================================
# Copyright (c) 2013-2018, Rethink Robotics Inc.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
import sys
import termios
import tty
from select import select
def getch(timeout=0.01):
"""
Retrieves a character from stdin.
Returns None if no character is available within the timeout.
Blocks if timeout < 0.
"""
# If this is being piped to, ignore non-blocking functionality
if not sys.stdin.isatty():
return sys.stdin.read(1)
fileno = sys.stdin.fileno()
old_settings = termios.tcgetattr(fileno)
ch = None
try:
tty.setraw(fileno)
rlist = [fileno]
if timeout >= 0:
[rlist, _, _] = select(rlist, [], [], timeout)
if fileno in rlist:
ch = sys.stdin.read(1)
except Exception as ex:
print("getch", ex)
raise OSError
finally:
termios.tcsetattr(fileno, termios.TCSADRAIN, old_settings)
return ch
================================================
FILE: franka_interface/src/franka_dataflow/wait_for.py
================================================
# /***************************************************************************
# Modified from: Rethink Robotics Intera SDK
#
# @package: franka_interface
# @metapackage: franka_ros_interface
# @author: Saif Sidhik <sxs1412@bham.ac.uk>
#
# **************************************************************************/
# /***************************************************************************
# Copyright (c) 2019-2021, Saif Sidhik
# Copyright (c) 2013-2018, Rethink Robotics Inc.
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
# http://www.apache.org/licenses/LICENSE-2.0
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
# **************************************************************************/
import errno
import rospy
def wait_for(test, timeout=1.0, raise_on_error=True, rate=100,
timeout_msg="timeout expired", body=None):
"""
Waits until some condition evaluates to true.
:param test: zero param function to be evaluated
:param timeout: max amount of time to wait. negative/inf for indefinitely
:param raise_on_error: raise or just return False
:param rate: the rate at which to check
:param timout_msg: message to supply to the timeout exception
:param body: optional function to execute while waiting
"""
end_time = rospy.get_time() + timeout
rate = rospy.Rate(rate)
notimeout = (timeout < 0.0) or timeout == float("inf")
while not test():
if rospy.is_shutdown():
if raise_on_error:
raise OSError(errno.ESHUTDOWN, "ROS Shutdown")
return False
elif (not notimeout) and (rospy.get_time() >= end_time):
if raise_on_error:
raise OSError(errno.ETIMEDOUT, timeout_msg)
rospy.loginfo(timeout_msg)
return False
if callable(body):
body()
rate.sleep()
return True
================================================
FILE: franka_interface/src/franka_interface/__init__.py
================================================
# /***************************************************************************
# Copyright (c) 2019-2021, Saif Sidhik
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
# http://www.apache.org/licenses/LICENSE-2.0
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
# **************************************************************************/
from .robot_params import RobotParams
from .arm import ArmInterface
from .gripper import GripperInterface
from .robot_enable import RobotEnable
================================================
FILE: franka_interface/src/franka_interface/arm.py
================================================
# /***************************************************************************
#
# @package: franka_interface
# @metapackage: franka_ros_interface
# @author: Saif Sidhik <sxs1412@bham.ac.uk>
#
# **************************************************************************/
# /***************************************************************************
# Copyright (c) 2019-2021, Saif Sidhik
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
# http://www.apache.org/licenses/LICENSE-2.0
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
# **************************************************************************/
"""
:info:
Inteface Class for Franka robot arm.
"""
from builtins import dict # for python2&3 efficient compatibility
from future.utils import iteritems # for python2&3 efficient compatibility
import enum
import rospy
import warnings
import quaternion
import numpy as np
from copy import deepcopy
from rospy_message_converter import message_converter
from franka_core_msgs.msg import JointCommand
from franka_core_msgs.msg import RobotState, EndPointState
from sensor_msgs.msg import JointState
from std_msgs.msg import Float64
import franka_dataflow
from .robot_params import RobotParams
from franka_moveit import PandaMoveGroupInterface
from franka_moveit.utils import create_pose_msg
from franka_tools import FrankaFramesInterface, FrankaControllerManagerInterface, JointTrajectoryActionClient, CollisionBehaviourInterface
class TipState():
def __init__(self, timestamp, pose, vel, O_effort, K_effort):
self.timestamp = timestamp
self._pose = pose
self._velocity = vel
self._effort = O_effort
self._effort_in_K_frame = K_effort
@property
def pose(self):
return self._pose
@property
def velocity(self):
return self._velocity
@property
def effort(self):
return self._effort
@property
def effort_in_K_frame(self):
return self._effort_in_K_frame
class ArmInterface(object):
"""
Interface Class for an arm of Franka Panda robot.
Constructor.
:type synchronous_pub: bool
:param synchronous_pub: designates the JointCommand Publisher
as Synchronous if True and Asynchronous if False.
Synchronous Publishing means that all joint_commands publishing to
the robot's joints will block until the message has been serialized
into a buffer and that buffer has been written to the transport
of every current Subscriber. This yields predicable and consistent
timing of messages being delivered from this Publisher. However,
when using this mode, it is possible for a blocking Subscriber to
prevent the joint_command functions from exiting. Unless you need exact
JointCommand timing, default to Asynchronous Publishing (False).
"""
# Containers
@enum.unique
class RobotMode(enum.IntEnum):
"""
Enum class for specifying and retrieving the current robot mode.
"""
# ----- access using parameters name or value
# ----- eg. RobotMode(0).name & RobotMode(0).value
# ----- or RobotMode['ROBOT_MODE_OTHER'].name & RobotMode['ROBOT_MODE_OTHER'].value
ROBOT_MODE_OTHER = 0
ROBOT_MODE_IDLE = 1
ROBOT_MODE_MOVE = 2
ROBOT_MODE_GUIDING = 3
ROBOT_MODE_REFLEX = 4
ROBOT_MODE_USER_STOPPED = 5
ROBOT_MODE_AUTOMATIC_ERROR_RECOVERY = 6
def __init__(self, synchronous_pub=False):
self._params = RobotParams()
self._ns = self._params.get_base_namespace()
self._joint_limits = self._params.get_joint_limits()
joint_names = self._joint_limits.joint_names
if not joint_names:
rospy.logerr("{}: Cannot detect joint names for arm on this "
"robot. Exiting Arm.init().".format(self.__class__.__name__))
return
self._joint_names = joint_names
self.name = self._params.get_robot_name()
self._joint_angle = dict()
self._joint_velocity = dict()
self._joint_effort = dict()
self._cartesian_pose = dict()
self._cartesian_velocity = dict()
self._cartesian_effort = dict()
self._stiffness_frame_effort = dict()
self._errors = dict()
self._collision_state = False
self._tip_states = None
self._jacobian = None
self._cartesian_contact = None
self._robot_mode = False
self._command_msg = JointCommand()
# neutral pose joint positions
self._neutral_pose_joints = self._params.get_neutral_pose()
self._frames_interface = FrankaFramesInterface()
try:
self._collision_behaviour_interface = CollisionBehaviourInterface()
except rospy.ROSException:
rospy.loginfo("{}: Collision Service Not found. It will not be possible to change collision behaviour of robot!".format(
self.__class__.__name__))
self._collision_behaviour_interface = None
self._ctrl_manager = FrankaControllerManagerInterface(
ns=self._ns, sim=self._params._in_sim)
self._speed_ratio = 0.15
self._F_T_NE = np.eye(1).flatten().tolist()
self._NE_T_EE = np.eye(1).flatten().tolist()
queue_size = None if synchronous_pub else 1
with warnings.catch_warnings():
warnings.simplefilter("ignore")
self._joint_command_publisher = rospy.Publisher(
self._ns + '/motion_controller/arm/joint_commands',
JointCommand,
tcp_nodelay=True,
queue_size=queue_size)
self._pub_joint_cmd_timeout = rospy.Publisher(
self._ns + '/motion_controller/arm/joint_command_timeout',
Float64,
latch=True,
queue_size=10)
self._robot_state_subscriber = rospy.Subscriber(
self._ns + '/custom_franka_state_controller/robot_state',
RobotState,
self._on_robot_state,
queue_size=1,
tcp_nodelay=True)
joint_state_topic = self._ns + '/custom_franka_state_controller/joint_states'
self._joint_state_sub = rospy.Subscriber(
joint_state_topic,
JointState,
self._on_joint_states,
queue_size=1,
tcp_nodelay=True)
self._cartesian_state_sub = rospy.Subscriber(
self._ns + '/custom_franka_state_controller/tip_state',
EndPointState,
self._on_endpoint_state,
queue_size=1,
tcp_nodelay=True)
rospy.on_shutdown(self._clean_shutdown)
err_msg = ("%s arm init failed to get current joint_states "
"from %s") % (self.name.capitalize(), joint_state_topic)
franka_dataflow.wait_for(lambda: len(list(self._joint_angle.keys())) > 0,
timeout_msg=err_msg, timeout=5.0)
err_msg = ("%s arm, init failed to get current tip_state "
"from %s") % (self.name.capitalize(), self._ns + 'tip_state')
franka_dataflow.wait_for(lambda: len(list(self._cartesian_pose.keys())) > 0,
timeout_msg=err_msg, timeout=5.0)
err_msg = ("%s arm, init failed to get current robot_state "
"from %s") % (self.name.capitalize(), self._ns + 'robot_state')
franka_dataflow.wait_for(lambda: self._jacobian is not None,
timeout_msg=err_msg, timeout=5.0)
# start moveit server with panda_link8 (flange) as the end-effector, unless it is in simulation. However, using move_to_cartesian_pose() method compensates for this and moves the robot's defined end-effector frame (EE frame; see set_EE_frame() and set_EE_at_frame()) to the target pose.
try:
self._movegroup_interface = PandaMoveGroupInterface(
use_panda_hand_link=True if self._params._in_sim else False)
except:
rospy.loginfo("{}: MoveGroup was not found! This is okay if moveit service is not required!".format(
self.__class__.__name__))
self._movegroup_interface = None
self.set_joint_position_speed(self._speed_ratio)
def _clean_shutdown(self):
self._joint_state_sub.unregister()
self._cartesian_state_sub.unregister()
self._pub_joint_cmd_timeout.unregister()
self._robot_state_subscriber.unregister()
self._joint_command_publisher.unregister()
def get_movegroup_interface(self):
"""
:return: the movegroup interface instance associated with the robot.
:rtype: franka_moveit.PandaMoveGroupInterface
"""
return self._movegroup_interface
def get_robot_params(self):
"""
:return: Useful parameters from the ROS parameter server.
:rtype: franka_interface.RobotParams
"""
return self._params
def get_joint_limits(self):
"""
Return the joint limits (defined in the parameter server)
:rtype: franka_core_msgs.msg.JointLimits
:return: JointLimits
"""
return self._joint_limits
def joint_names(self):
"""
Return the names of the joints for the specified limb.
:rtype: [str]
:return: ordered list of joint names from proximal to distal (i.e. shoulder to wrist).
"""
return self._joint_names
def _on_joint_states(self, msg):
for idx, name in enumerate(msg.name):
if name in self._joint_names:
self._joint_angle[name] = msg.position[idx]
self._joint_velocity[name] = msg.velocity[idx]
self._joint_effort[name] = msg.effort[idx]
def _on_robot_state(self, msg):
self._robot_mode = self.RobotMode(msg.robot_mode)
self._robot_mode_ok = (self._robot_mode.value != self.RobotMode.ROBOT_MODE_REFLEX) and (
self._robot_mode.value != self.RobotMode.ROBOT_MODE_USER_STOPPED)
self._jacobian = np.asarray(msg.O_Jac_EE).reshape(6, 7, order='F')
self._cartesian_velocity = {
'linear': np.asarray([msg.O_dP_EE[0], msg.O_dP_EE[1], msg.O_dP_EE[2]]),
'angular': np.asarray([msg.O_dP_EE[3], msg.O_dP_EE[4], msg.O_dP_EE[5]])}
self._cartesian_contact = msg.cartesian_contact
self._cartesian_collision = msg.cartesian_collision
self._joint_contact = msg.joint_contact
self._joint_collision = msg.joint_collision
if not self._params._in_sim:
self._F_T_NE = msg.F_T_NE # should be constant normally
self._NE_T_EE = msg.NE_T_EE
self._F_T_EE = msg.F_T_EE
if self._frames_interface:
self._frames_interface._update_frame_data(
self._NE_T_EE, msg.EE_T_K)
self._joint_inertia = np.asarray(
msg.mass_matrix).reshape(7, 7, order='F')
self.q_d = msg.q_d
self.dq_d = msg.dq_d
self._gravity = np.asarray(msg.gravity)
self._coriolis = np.asarray(msg.coriolis)
self._errors = message_converter.convert_ros_message_to_dictionary(
msg.current_errors)
def coriolis_comp(self):
"""
Return coriolis compensation torques. Useful for compensating coriolis when
performing direct torque control of the robot.
:rtype: np.ndarray
:return: 7D joint torques compensating for coriolis.
"""
return self._coriolis
def gravity_comp(self):
"""
Return gravity compensation torques.
:rtype: np.ndarray
:return: 7D joint torques compensating for gravity.
"""
return self._gravity
def get_robot_status(self):
"""
Return dict with all robot status information.
:rtype: dict
:return: ['robot_mode' (RobotMode object), 'robot_status' (bool), 'errors' (dict() of errors and their truth value), 'error_in_curr_status' (bool)]
"""
return {'robot_mode': self._robot_mode, 'robot_status': self._robot_mode_ok, 'errors': self._errors, 'error_in_current_state': self.error_in_current_state()}
def in_safe_state(self):
"""
Return True if the specified limb is in safe state (no collision, reflex, errors etc.).
:rtype: bool
:return: True if the arm is in safe state, False otherwise.
"""
return self._robot_mode_ok and not self.error_in_current_state()
def error_in_current_state(self):
"""
Return True if the specified limb has experienced an error.
:rtype: bool
:return: True if the arm has error, False otherwise.
"""
return not all([e == False for e in list(self._errors.values())])
def what_errors(self):
"""
Return list of error messages if there is error in robot state
:rtype: [str]
:return: list of names of current errors in robot state
"""
return [e for e in self._errors if self._errors[e] == True] if self.error_in_current_state() else None
def _on_endpoint_state(self, msg):
cart_pose_trans_mat = np.asarray(msg.O_T_EE).reshape(4, 4, order='F')
self._cartesian_pose = {
'position': cart_pose_trans_mat[:3, 3],
'orientation': quaternion.from_rotation_matrix(cart_pose_trans_mat[:3, :3]),
'ori_mat': cart_pose_trans_mat[:3,:3]}
self._cartesian_effort = {
'force': np.asarray([msg.O_F_ext_hat_K.wrench.force.x,
msg.O_F_ext_hat_K.wrench.force.y,
msg.O_F_ext_hat_K.wrench.force.z]),
'torque': np.asarray([msg.O_F_ext_hat_K.wrench.torque.x,
msg.O_F_ext_hat_K.wrench.torque.y,
msg.O_F_ext_hat_K.wrench.torque.z])
}
self._stiffness_frame_effort = {
'force': np.asarray([msg.K_F_ext_hat_K.wrench.force.x,
msg.K_F_ext_hat_K.wrench.force.y,
msg.K_F_ext_hat_K.wrench.force.z]),
'torque': np.asarray([msg.K_F_ext_hat_K.wrench.torque.x,
msg.K_F_ext_hat_K.wrench.torque.y,
msg.K_F_ext_hat_K.wrench.torque.z])
}
self._tip_states = TipState(msg.header.stamp, deepcopy(self._cartesian_pose), deepcopy(
self._cartesian_velocity), deepcopy(self._cartesian_effort), deepcopy(self._stiffness_frame_effort))
def joint_angle(self, joint):
"""
Return the requested joint angle.
:type joint: str
:param joint: name of a joint
:rtype: float
:return: angle in radians of individual joint
"""
return self._joint_angle[joint]
def joint_angles(self):
"""
Return all joint angles.
:rtype: dict({str:float})
:return: unordered dict of joint name Keys to angle (rad) Values
"""
return deepcopy(self._joint_angle)
def joint_ordered_angles(self):
"""
Return all joint angles.
:rtype: [float]
:return: joint angles (rad) orded by joint_names from proximal to distal (i.e. shoulder to wrist).
"""
return [self._joint_angle[name] for name in self._joint_names]
def joint_velocity(self, joint):
"""
Return the requested joint velocity.
:type joint: str
:param joint: name of a joint
:rtype: float
:return: velocity in radians/s of individual joint
"""
return self._joint_velocity[joint]
def joint_velocities(self):
"""
Return all joint velocities.
:rtype: dict({str:float})
:return: unordered dict of joint name Keys to velocity (rad/s) Values
"""
return deepcopy(self._joint_velocity)
def joint_effort(self, joint):
"""
Return the requested joint effort.
:type joint: str
:param joint: name of a joint
:rtype: float
:return: effort in Nm of individual joint
"""
return self._joint_effort[joint]
def joint_efforts(self):
"""
Return all joint efforts.
:rtype: dict({str:float})
:return: unordered dict of joint name Keys to effort (Nm) Values
"""
return deepcopy(self._joint_effort)
def endpoint_pose(self):
"""
Return Cartesian endpoint pose {position, orientation}.
:rtype: dict({str:np.ndarray (shape:(3,)), str:quaternion.quaternion})
:return: position and orientation as named tuples in a dict
- 'position': np.array of x, y, z
- 'orientation': quaternion x,y,z,w in quaternion format
"""
return deepcopy(self._cartesian_pose)
def endpoint_velocity(self):
"""
Return Cartesian endpoint twist {linear, angular}.
:rtype: dict({str:np.ndarray (shape:(3,)),str:np.ndarray (shape:(3,))})
:return: linear and angular velocities as named tuples in a dict
- 'linear': np.array of x, y, z
- 'angular': np.array of x, y, z (angular velocity along the axes)
"""
return deepcopy(self._cartesian_velocity)
def endpoint_effort(self, in_base_frame=True):
"""
Return Cartesian endpoint wrench {force, torque}.
:param in_base_frame: if True, returns end-effector effort with respect to base frame, else in stiffness frame [default: True]
:type in_base_frame: bool
:rtype: dict({str:np.ndarray (shape:(3,)),str:np.ndarray (shape:(3,))})
:return: force and torque at endpoint as named tuples in a dict in the base frame of the robot or in the stiffness frame (wrist)
- 'force': Cartesian force on x,y,z axes in np.ndarray format
- 'torque': Torque around x,y,z axes in np.ndarray format
"""
return deepcopy(self._cartesian_effort) if in_base_frame else deepcopy(self._stiffness_frame_effort)
def exit_control_mode(self, timeout=5, velocity_tolerance=1e-2):
"""
Clean exit from advanced control modes (joint torque or velocity).
Resets control to joint position mode with current positions until
further advanced control commands are sent to the robot.
.. note:: In normal cases, this method is not required as the
interface automatically switches to position control mode if
advanced control commands (velocity/torque) are not sent at
regular intervals. Therefore it is enough to stop sending the
commands to disable advanced control modes.
.. note:: In sim, this method does nothing.
:type timeout: float
:param timeout: seconds to wait for robot to stop moving before giving up [default: 5]
:type velocity_tolerance: float
:param velocity_tolerance: tolerance
"""
if self._params._in_sim: return
self.set_command_timeout(0.05)
rospy.sleep(0.5)
def check_stop():
return np.allclose(np.asarray(list(self._joint_velocity.values())), 0., atol=velocity_tolerance)
rospy.loginfo("{}: Waiting for robot to stop moving to exit control mode...".format(
self.__class__.__name__))
franka_dataflow.wait_for(
test=lambda: check_stop(),
timeout=timeout,
timeout_msg="{}: FAILED to exit control mode! The robot may be still moving. Controllers might not switch correctly".format(
self.__class__.__name__),
rate=20,
raise_on_error=False
)
rospy.loginfo("{}: Done. Setting position control target to current position.".format(
self.__class__.__name__))
self.set_joint_positions(self.joint_angles())
def tip_states(self):
"""
Return Cartesian endpoint state for a given tip name
:rtype: TipState object
:return: pose, velocity, effort, effort_in_K_frame
"""
return deepcopy(self._tip_states)
def joint_inertia_matrix(self):
"""
Returns the current joint inertia matrix given by libfranka.
:return: joint inertia matrix (7,7)
:rtype: np.ndarray [7x7]
"""
return deepcopy(self._joint_inertia)
def zero_jacobian(self):
"""
Returns the current jacobian matrix given by libfranka.
:return: end-effector jacobian (6,7)
:rtype: np.ndarray [6x7]
"""
return deepcopy(self._jacobian)
def set_command_timeout(self, timeout):
"""
Set the timeout in seconds for the joint controller. Advanced
control commands (vel, torque) should be sent within this
time, or else the controller will switch back to default
control mode.
:type timeout: float
:param timeout: timeout in seconds
"""
self._pub_joint_cmd_timeout.publish(Float64(timeout))
def set_joint_position_speed(self, speed=0.3):
"""
Set ratio of max joint speed to use during joint position
moves (only for move_to_joint_positions).
Set the proportion of maximum controllable velocity to use
during joint position control execution. The default ratio
is `0.3`, and can be set anywhere from [0.0-1.0] (clipped).
Once set, a speed ratio will persist until a new execution
speed is set.
:type speed: float
:param speed: ratio of maximum joint speed for execution
default= 0.3; range= [0.0-1.0]
"""
if speed > 0.3:
rospy.logwarn("{}: Setting speed above 0.3 could be risky!! Be extremely careful.".format(
self.__class__.__name__))
if self._movegroup_interface:
self._movegroup_interface.set_velocity_scale(speed * 2)
self._speed_ratio = speed
def set_joint_positions(self, positions):
"""
Commands the joints of this limb to the specified positions.
:type positions: dict({str:float}
:param positions: dict of {'joint_name':joint_position,}
"""
self._command_msg.names = self._joint_names
self._command_msg.position = [positions[j] for j in self._joint_names]
self._command_msg.mode = JointCommand.POSITION_MODE
self._command_msg.header.stamp = rospy.Time.now()
self._joint_command_publisher.publish(self._command_msg)
def set_joint_velocities(self, velocities):
"""
Commands the joints of this limb to the specified velocities.
:type velocities: dict({str:float})
:param velocities: dict of {'joint_name':joint_velocity,}
"""
self._command_msg.names = self._joint_names
self._command_msg.velocity = [velocities[j] for j in self._joint_names]
self._command_msg.mode = JointCommand.VELOCITY_MODE
self._command_msg.header.stamp = rospy.Time.now()
self._joint_command_publisher.publish(self._command_msg)
def set_joint_torques(self, torques):
"""
Commands the joints of this limb with the specified torques.
:type torques: dict({str:float})
:param torques: dict of {'joint_name':joint_torque,}
"""
self._command_msg.names = self._joint_names
self._command_msg.effort = [torques[j] for j in self._joint_names]
self._command_msg.mode = JointCommand.TORQUE_MODE
self._command_msg.header.stamp = rospy.Time.now()
self._joint_command_publisher.publish(self._command_msg)
def set_joint_positions_velocities(self, positions, velocities):
"""
Commands the joints of this limb using specified positions and velocities using impedance control.
Command at time t is computed as:
:math:`u_t= coriolis\_factor * coriolis\_t + K\_p * (positions - curr\_positions) + K\_d * (velocities - curr\_velocities)`
:type positions: [float]
:param positions: desired joint positions as an ordered list corresponding to joints given by self.joint_names()
:type velocities: [float]
:param velocities: desired joint velocities as an ordered list corresponding to joints given by self.joint_names()
"""
self._command_msg.names = self._joint_names
self._command_msg.position = positions
self._command_msg.velocity = velocities
self._command_msg.mode = JointCommand.IMPEDANCE_MODE
self._command_msg.header.stamp = rospy.Time.now()
self._joint_command_publisher.publish(self._command_msg)
def has_collided(self):
"""
Returns true if either joint collision or cartesian collision is detected.
Collision thresholds can be set using instance of :py:class:`franka_tools.CollisionBehaviourInterface`.
"""
return any(self._joint_collision) or any(self._cartesian_collision)
def move_to_neutral(self, timeout=15.0, speed=0.15):
"""
Command the Limb joints to a predefined set of "neutral" joint angles.
From rosparam /franka_control/neutral_pose.
:type timeout: float
:param timeout: seconds to wait for move to finish [15]
:type speed: float
:param speed: ratio of maximum joint speed for execution
default= 0.15; range= [0.0-1.0]
"""
self.set_joint_position_speed(speed)
self.move_to_joint_positions(self._neutral_pose_joints, timeout)
def move_to_joint_positions(self, positions, timeout=10.0,
threshold=0.00085,
test=None, use_moveit=True):
"""
(Blocking) Commands the limb to the provided positions.
Waits until the reported joint state matches that specified.
This function uses a low-pass filter using JointTrajectoryService
to smooth the movement or optionally uses MoveIt! to plan and
execute a trajectory.
:type positions: dict({str:float})
:param positions: joint_name:angle command
:type timeout: float
:param timeout: seconds to wait for move to finish [15]
:type threshold: float
:param threshold: position threshold in radians across each joint when
move is considered successful [0.00085]
:param test: optional function returning True if motion must be aborted
:type use_moveit: bool
:param use_moveit: if set to True, and movegroup interface is available,
move to the joint positions using moveit planner.
"""
curr_controller = self._ctrl_manager.set_motion_controller(
self._ctrl_manager.joint_trajectory_controller)
if use_moveit and self._movegroup_interface:
self._movegroup_interface.go_to_joint_positions(
[positions[n] for n in self._joint_names], tolerance=threshold)
else:
if use_moveit:
rospy.logwarn("{}: MoveGroupInterface was not found! Using JointTrajectoryActionClient instead.".format(
self.__class__.__name__))
min_traj_dur = 0.5
traj_client = JointTrajectoryActionClient(
joint_names=self.joint_names())
traj_client.clear()
dur = []
for j in range(len(self._joint_names)):
dur.append(max(abs(positions[self._joint_names[j]] - self._joint_angle[self._joint_names[j]]
) / self._joint_limits.velocity[j], min_traj_dur))
traj_client.add_point(
positions=[self._joint_angle[n] for n in self._joint_names], time=0.0001)
traj_client.add_point(positions=[
positions[n] for n in self._joint_names], time=max(dur)/self._speed_ratio)
def genf(joint, angle):
def joint_diff():
return abs(angle - self._joint_angle[joint])
return joint_diff
diffs = [genf(j, a) for j, a in list(iteritems(positions)) if
j in self._joint_angle]
fail_msg = "{}: {} limb failed to reach commanded joint positions.".format(
self.__class__.__name__, self.name.capitalize())
def test_collision():
if self.has_collided():
rospy.logerr(' '.join(["Collision detected.", fail_msg]))
return True
return False
traj_client.start() # send the trajectory action request
# traj_client.wait(timeout = timeout)
franka_dataflow.wait_for(
test=lambda: test_collision() or traj_client.result() is not None or
(callable(test) and test() == True) or
(all(diff() < threshold for diff in diffs)),
timeout=timeout,
timeout_msg=fail_msg,
rate=100,
raise_on_error=False
)
res = traj_client.result()
if res is not None and res.error_code:
rospy.loginfo("Trajectory Server Message: {}".format(res))
rospy.sleep(0.5)
self._ctrl_manager.set_motion_controller(curr_controller)
rospy.loginfo("{}: Trajectory controlling complete".format(
self.__class__.__name__))
def get_flange_pose(self, pos=None, ori=None):
"""
Get the pose of flange (panda_link8) given the pose of the end-effector frame.
.. note:: In sim, this method does nothing.
:param pos: position of the end-effector frame in the robot's base frame, defaults to current end-effector position
:type pos: np.ndarray, optional
:param ori: orientation of the end-effector frame, defaults to current end-effector orientation
:type ori: quaternion.quaternion, optional
:return: corresponding flange frame pose in the robot's base frame
:rtype: np.ndarray, quaternion.quaternion
"""
if pos is None:
pos = self._cartesian_pose['position']
if ori is None:
ori = self._cartesian_pose['orientation']
if self._params._in_sim:
return pos, ori
# get corresponding flange frame pose using transformation matrix
F_T_EE = np.asarray(self._F_T_EE).reshape(4, 4, order="F")
mat = quaternion.as_rotation_matrix(ori)
new_ori = mat.dot(F_T_EE[:3,:3].T)
new_pos = pos - new_ori.dot(F_T_EE[:3, 3])
return new_pos, quaternion.from_rotation_matrix(new_ori)
def move_to_cartesian_pose(self, pos, ori=None, use_moveit=True):
"""
Move robot end-effector to specified cartesian pose using MoveIt! (also avoids obstacles if they are defined using :py:class:`franka_moveit.ExtendedPlanningSceneInterface`)
:param pos: target end-effector position
:type pos: [float] or np.ndarray
:param ori: target orientation quaternion for end-effector, defaults to current ori
:type ori: quaternion.quaternion or [float] (quaternion in w,x,y,z order), optional
:param use_moveit: Flag for using MoveIt (redundant for now; only works if set to True), defaults to True
:type use_moveit: bool, optional
"""
if not use_moveit or self._movegroup_interface is None:
rospy.logerr("{}: MoveGroupInterface was not found! Aborting cartesian planning.".format(
self.__class__.__name__))
return
if ori is None:
ori = self._cartesian_pose['orientation']
self.get_flange_pose(pos, ori)
curr_controller = self._ctrl_manager.set_motion_controller(
self._ctrl_manager.joint_trajectory_controller)
## == Plan avoids defined scene obstacles ==
self._movegroup_interface.go_to_cartesian_pose(
create_pose_msg(*self.get_flange_pose(pos, ori)))
## =========================================
## == does not avoid scene obstacles ==
# plan, _ = self._movegroup_interface.plan_cartesian_path(
# [create_pose_msg(pos, ori)])
# self._movegroup_interface.execute_plan(plan)
## ====================================
rospy.sleep(0.5)
self._ctrl_manager.set_motion_controller(curr_controller)
rospy.loginfo("{}: Trajectory controlling complete".format(
self.__class__.__name__))
def pause_controllers_and_do(self, func, *args, **kwargs):
"""
Temporarily stops all active controllers and calls the provided function
before restarting the previously active controllers.
:param func: the function to be called
:type func: callable
"""
assert callable(
func), "{}: Invalid argument provided to ArmInterface->pause_controllers_and_do. Argument 1 should be callable.".format(self.__class__.__name__)
active_controllers = self._ctrl_manager.list_active_controllers(
only_motion_controllers=True)
rospy.loginfo("{}: Stopping motion controllers temporarily...".format(
self.__class__.__name__))
for ctrlr in active_controllers:
self._ctrl_manager.stop_controller(ctrlr.name)
rospy.sleep(1.)
retval = func(*args, **kwargs)
rospy.sleep(1.)
rospy.loginfo("{}: Restarting previously active motion controllers.".format(
self.__class__.__name__))
for ctrlr in active_controllers:
self._ctrl_manager.start_controller(ctrlr.name)
rospy.sleep(1.)
rospy.loginfo("{}: Controllers restarted.".format(
self.__class__.__name__))
return retval
def reset_EE_frame(self):
"""
Reset EE frame to default. (defined by
FrankaFramesInterface.DEFAULT_TRANSFORMATIONS.EE_FRAME
global variable defined in :py:class:`franka_tools.FrankaFramesInterface`
source code). By default, this resets to align with the nominal-end effector
frame (F_T_NE) in the flange frame.
:rtype: [bool, str]
:return: [success status of service request, error msg if any]
"""
if self._frames_interface:
if self._frames_interface.EE_frame_is_reset():
rospy.loginfo("{}: EE Frame already reset".format(
self.__class__.__name__))
return True
return self.pause_controllers_and_do(self._frames_interface.reset_EE_frame)
else:
rospy.logwarn("{}: Frames changing not available in simulated environment".format(
self.__class__.__name__))
return False
def set_EE_frame(self, frame):
"""
Set new EE frame based on the transformation given by 'frame', which is the
transformation matrix defining the new desired EE frame with respect to the
nominal end-effector frame (NE_T_EE).
Motion controllers are stopped and restarted for switching.
:type frame: [float (16,)] / np.ndarray (4x4)
:param frame: transformation matrix of new EE frame wrt nominal end-effector frame (column major)
:rtype: [bool, str]
:return: [success status of service request, error msg if any]
"""
if self._frames_interface:
if self._frames_interface.frames_are_same(self._frames_interface.get_EE_frame(as_mat=True), frame):
rospy.loginfo("{}: EE Frame already at the target frame.".format(
self.__class__.__name__))
return True
return self.pause_controllers_and_do(self._frames_interface.set_EE_frame, frame)
else:
rospy.logwarn("{}: Frames changing not available in simulated environment".format(
self.__class__.__name__))
return False
def set_EE_at_frame(self, frame_name, timeout=5.0):
"""
Set new EE frame to the same frame as the link frame given by 'frame_name'.
Motion controllers are stopped and restarted for switching
:type frame_name: str
:param frame_name: desired tf frame name in the tf tree
:rtype: [bool, str]
:return: [success status of service request, error msg if any]
"""
if self._frames_interface:
if not self._frames_interface.EE_frame_already_set(self._frames_interface.get_link_tf(frame_name)):
return self.pause_controllers_and_do(self._frames_interface.set_EE_at_frame, frame_name=frame_name, timeout=timeout)
else:
rospy.logwarn("{}: Frames changing not available in simulated environment".format(
self.__class__.__name__))
return False
def set_collision_threshold(self, cartesian_forces=None, joint_torques=None):
"""
Set Force Torque thresholds for deciding robot has collided.
:return: True if service call successful, False otherwise
:rtype: bool
:param cartesian_forces: Cartesian force threshold for collision detection [x,y,z,R,P,Y] (robot motion stops if violated)
:type cartesian_forces: [float] size 6
:param joint_torques: Joint torque threshold for collision (robot motion stops if violated)
:type joint_torques: [float] size 7
"""
if self._collision_behaviour_interface:
return self._collision_behaviour_interface.set_collision_threshold(joint_torques=joint_torques, cartesian_forces=cartesian_forces)
else:
rospy.logwarn("No CollisionBehaviourInterface object found!")
def get_controller_manager(self):
"""
:return: the FrankaControllerManagerInterface instance associated with the robot.
:rtype: franka_tools.FrankaControllerManagerInterface
"""
return self._ctrl_manager
def get_frames_interface(self):
"""
:return: the FrankaFramesInterface instance associated with the robot.
:rtype: franka_tools.FrankaFramesInterface
"""
return self._frames_interface
if __name__ == '__main__':
rospy.init_node('test_fri')
r = Arm()
# rate = rospy.Rate(10)
# while not rospy.is_shutdown():
# rate.sleep()
================================================
FILE: franka_interface/src/franka_interface/gripper.py
================================================
# /***************************************************************************
#
# @package: franka_interface
# @metapackage: franka_ros_interface
# @author: Saif Sidhik <sxs1412@bham.ac.uk>
#
# **************************************************************************/
# /***************************************************************************
# Copyright (c) 2019-2021, Saif Sidhik
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
# http://www.apache.org/licenses/LICENSE-2.0
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
# **************************************************************************/
"""
:info:
Inteface Class for Franka gripper.
"""
from builtins import dict # for python2&3 efficient compatibility
import rospy
import actionlib
import franka_dataflow
from copy import deepcopy
from sensor_msgs.msg import JointState
from franka_gripper.msg import ( GraspAction, GraspGoal,
HomingAction, HomingGoal,
MoveAction, MoveGoal,
StopAction, StopGoal,
GraspEpsilon )
class GripperInterface(object):
"""
Interface class for the gripper on the Franka Panda robot.
:param gripper_joint_names: Names of the finger joints
:param ns: base namespace of interface ('frank_ros_interface'/'panda_simulator')
:param calibrate: Attempts to calibrate the gripper when initializing class (defaults True)
:type calibrate: bool
:type gripper_joint_names: [str]
:type ns: str
"""
def __init__(self, gripper_joint_names = ('panda_finger_joint1', 'panda_finger_joint2'), calibrate = False, **kwargs):
"""
Constructor.
"""
self.name = '/franka_gripper'
ns = self.name +'/'
self._joint_positions = dict()
self._joint_names = gripper_joint_names
self._joint_velocity = dict()
self._joint_effort = dict()
self._joint_states_state_sub = rospy.Subscriber(ns + 'joint_states', JointState, self._joint_states_callback, queue_size = 1, tcp_nodelay = True)
self._exists = False
# ----- Initial test to see if gripper is loaded
try:
rospy.get_param("/franka_gripper/robot_ip")
except KeyError:
rospy.loginfo("FrankaGripper: could not detect gripper.")
return
except (socket.error, socket.gaierror):
print ("Failed to connect to the ROS parameter server!\n"
"Please check to make sure your ROS networking is "
"properly configured:\n")
sys.exit()
# ----- Wait for the gripper device status to be true
if not franka_dataflow.wait_for(lambda: len(list(self._joint_positions.keys())) > 0, timeout=2.0, timeout_msg=("FrankaGripper: Failed to get gripper joint positions. Assuming no gripper attached to robot."), raise_on_error = False ):
return
self._exists = True
self._gripper_speed = 0.05
self._homing_action_client = actionlib.SimpleActionClient("{}homing".format(ns), HomingAction)
self._grasp_action_client = actionlib.SimpleActionClient("{}grasp".format(ns), GraspAction)
self._move_action_client = actionlib.SimpleActionClient("{}move".format(ns), MoveAction)
self._stop_action_client = actionlib.SimpleActionClient("{}stop".format(ns), StopAction)
rospy.loginfo("GripperInterface: Waiting for gripper action servers... ")
self._homing_action_client.wait_for_server()
self._grasp_action_client.wait_for_server()
self._move_action_client.wait_for_server()
self._stop_action_client.wait_for_server()
rospy.loginfo("GripperInterface: Gripper action servers found! ")
self.MIN_FORCE = 0.01
self.MAX_FORCE = 50 # documentation says upto 70N is possible as continuous force (max upto 140N)
self.MIN_WIDTH = 0.0001
self.MAX_WIDTH = 0.2
if calibrate:
self.calibrate()
@property
def exists(self):
"""
Check if a gripper was identified as connected to the robot.
:return: True if gripper was detected, False otherwise
:rtype: bool
"""
return self._exists
def set_velocity(self, value):
"""
Set default value for gripper joint motions. Used for move and grasp commands.
:param value: speed value [m/s]
:type value: float
"""
assert self.MIN_WIDTH <= value <= self.MAX_WIDTH, "GripperInterface: Invalid speed request for gripper joints. Should be within {} and {}.".format(self.MIN_WIDTH, self.MAX_WIDTH)
self._gripper_speed = value
def _joint_states_callback(self, msg):
for idx, name in enumerate(msg.name):
if name in self._joint_names:
self._joint_positions[name] = msg.position[idx]
self._joint_velocity[name] = msg.velocity[idx]
self._joint_effort[name] = msg.effort[idx]
def joint_names(self):
"""
Return the names of the joints for the specified limb.
:rtype: [str]
:return: ordered list of joint names.
"""
return self._joint_names
def joint_position(self, joint):
"""
Return the requested joint position.
:param joint: name of a joint
:type joint: str
:rtype: float
:return: position individual joint
"""
return self._joint_positions[joint]
def joint_positions(self):
"""
Return all joint positions.
:rtype: dict({str:float})
:return: unordered dict of joint name Keys to pos
"""
return deepcopy(self._joint_positions)
def joint_ordered_positions(self):
"""
Return all joint positions.
:rtype: [double]
:return: joint positions ordered by joint_names.
"""
return [self._joint_positions[name] for name in self._joint_names]
def joint_velocity(self, joint):
"""
Return the requested joint velocity.
:param joint: name of a joint
:type joint: str
:rtype: float
:return: velocity in radians/s of individual joint
"""
return self._joint_velocity[joint]
def joint_velocities(self):
"""
Return all joint velocities.
:rtype: dict({str:float})
:return: unordered dict of joint name Keys to velocity (rad/s) Values
"""
return deepcopy(self._joint_velocity)
def joint_ordered_velocities(self):
"""
Return all joint velocities.
:rtype: [double]
:return: joint velocities ordered by joint_names.
"""
return [self._joint_velocity[name] for name in self._joint_names]
def joint_effort(self, joint):
"""
Return the requested joint effort.
:param joint: name of a joint
:type joint: str
:rtype: float
:return: effort in Nm of individual joint
"""
return self._joint_effort[joint]
def joint_efforts(self):
"""
Return all joint efforts.
:rtype: dict({str:float})
:return: unordered dict of joint name Keys to effort (Nm) Values
"""
return deepcopy(self._joint_effort)
def joint_ordered_efforts(self):
"""
Return all joint efforts.
:rtype: [double]
:return: joint efforts ordered by joint_names.
"""
return [self._joint_effort[name] for name in self._joint_names]
def _active_cb(self):
rospy.logdebug("GripperInterface: '{}' request active.".format(self._caller))
def _feedback_cb(self, msg):
rospy.logdebug("GripperInterface: '{}' request feedback: \n\t{}".format(self._caller,msg))
def _done_cb(self, status, result):
rospy.logdebug("GripperInterface: '{}' complete. Result: \n\t{}".format(self._caller, result))
def home_joints(self, wait_for_result = False):
"""
Performs homing of the gripper.
After changing the gripper fingers, a homing needs to be done.
This is needed to estimate the maximum grasping width.
:param wait_for_result: if True, this method will block till response is
recieved from server
:type wait_for_result: bool
:return: success
:rtype: bool
"""
self._caller = "home_joints"
goal = HomingGoal()
self._homing_action_client.send_goal(goal, done_cb =self._done_cb, active_cb = self._active_cb, feedback_cb = self._feedback_cb)
if wait_for_result:
result = self._homing_action_client.wait_for_result(rospy.Duration(15.))
return result
return True
def open(self):
"""
Open gripper to max possible width.
:return: True if command was successful, False otherwise.
:rtype: bool
"""
self._caller = "open gripper"
return self.move_joints(0.2)
def close(self):
"""
close gripper to till collision is detected.
.. note:: This method is known to have a bug where it sometimes opens
the gripper when it is already closed. Use :py:meth:`close` with
argument 0 (zero) if you want to close it, or use :py:meth:`grasp`
wherever possible.
.. note:: This is not exactly doing what it should. The behaviour is
faked by catching the error thrown when trying to grasp a very small
object with a very small force. Since the gripper will actually hit the
object before it reaches the commanded width, we catch the feedback
and send the gripper stop command to stop it where it is.
:return: True if command was successful, False otherwise.
:rtype: bool
"""
def cb( _, result):
if not result.success:
self.stop_action()
self._caller = "close gripper"
return self.grasp(0.0, 0.1, cb = cb)
def calibrate(self):
return self.home_joints(wait_for_result = True)
def move_joints(self, width, speed = None, wait_for_result = True):
"""
Moves the gripper fingers to a specified width.
:param width: Intended opening width. [m]
:param speed: Closing speed. [m/s]
:param wait_for_result: if True, this method will block till response is
recieved from server
:type width: float
:type speed: float
:type wait_for_result: bool
:return: True if command was successful, False otherwise.
:rtype: bool
"""
self._caller = "move_joints"
goal = MoveGoal()
if not speed:
speed = self._gripper_speed
goal.width = width
goal.speed = speed
self._move_action_client.send_goal(goal, done_cb =self._done_cb, active_cb = self._active_cb, feedback_cb = self._feedback_cb)
if wait_for_result:
result = self._move_action_client.wait_for_result(rospy.Duration(15.))
return result
return True
def stop_action(self):
"""
Stops a currently running gripper move or grasp.
:return: True if command was successful, False otherwise.
:rtype: bool
"""
self._caller = "stop_action"
goal = StopGoal()
self._stop_action_client.send_goal(goal, done_cb =self._done_cb, active_cb = self._active_cb, feedback_cb = self._feedback_cb)
result = self._stop_action_client.wait_for_result(rospy.Duration(15.))
return result
def grasp(self, width, force, speed = None, epsilon_inner = 0.005, epsilon_outer = 0.005,wait_for_result = True, cb = None):
"""
Grasps an object.
An object is considered grasped if the distance :math:`d` between the gripper fingers satisfies
:math:`(width - epsilon\_inner) < d < (width + epsilon\_outer)`.
:param width: Size of the object to grasp. [m]
:param speed: Closing speed. [m/s]
:param force: Grasping force. [N]
:param epsilon_inner: Maximum tolerated deviation when the actual grasped width is smaller
than the commanded grasp width.
:param epsilon_outer: Maximum tolerated deviation when the actual grasped width is wider
than the commanded grasp width.
:param cb: Optional callback function to use when the service call is done
:type width: float
:type speed: float
:type force: float
:type epsilon_inner: float
:type epsilon_outer: float
:return: True if an object has been grasped, false otherwise.
:rtype: bool
"""
self._caller = "grasp_action"
if not speed:
speed = self._gripper_speed
goal = GraspGoal()
goal.width = width
goal.speed = speed
goal.force = force
goal.epsilon = GraspEpsilon(inner = epsilon_inner, outer = epsilon_outer)
if not cb:
cb = self._done_cb
self._grasp_action_client.send_goal(goal, done_cb = cb, active_cb = self._active_cb, feedback_cb = self._feedback_cb)
if wait_for_result:
result = self._grasp_action_client.wait_for_result(rospy.Duration(15.))
return result
return True
================================================
FILE: franka_interface/src/franka_interface/robot_enable.py
================================================
# /***************************************************************************
#
# @package: franka_interface
# @metapackage: franka_ros_interface
# @author: Saif Sidhik <sxs1412@bham.ac.uk>
#
# **************************************************************************/
# /***************************************************************************
# Copyright (c) 2019-2021, Saif Sidhik
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
# http://www.apache.org/licenses/LICENSE-2.0
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
# **************************************************************************/
"""
:info:
Wrapper class for enabling and resetting robot state.
"""
import rospy
from threading import Lock
from franka_msgs.msg import ErrorRecoveryActionGoal
from franka_core_msgs.msg import RobotState
import franka_dataflow
from .robot_params import RobotParams
class RobotEnable(object):
"""
Class RobotEnable - simple control/status wrapper around robot state
enable() - enable all joints
disable() - disable all joints
reset() - reset all joints, reset all jrcp faults, disable the robot
stop() - stop the robot, similar to hitting the e-stop button
:param robot_params: A RobotParams instance (optional)
:type robot_params: RobotParams
"""
param_lock = Lock()
def __init__(self, robot_params = None):
"""
"""
if robot_params:
self._params = robot_params
else:
self._params = RobotParams()
self._ns = self._params.get_base_namespace()
self._enabled = None
state_topic = '{}/custom_franka_state_controller/robot_state'.format(self._ns)
self._state_sub = rospy.Subscriber(state_topic,
RobotState,
self._state_callback
)
franka_dataflow.wait_for(
lambda: not self._enabled is None,
timeout=5.0,
timeout_msg=("Failed to get robot state on %s" %
(state_topic,)),
)
def _state_callback(self, msg):
self._enabled = (msg.robot_mode != 4)
def is_enabled(self):
"""
Return status of robot
:return: True if enabled, False otherwise
:rtype: bool
"""
return self._enabled
def _toggle_enabled(self, status):
pub = rospy.Publisher('{}/franka_control/error_recovery/goal'.format(self._ns), ErrorRecoveryActionGoal,
queue_size=10)
if self._enabled == status:
rospy.loginfo("Robot is already %s"%self.state())
franka_dataflow.wait_for(
test=lambda: self._enabled == status,
timeout=5.0,
timeout_msg=("Failed to %sable robot" %
('en' if status else 'dis',)),
body=lambda: pub.publish(ErrorRecoveryActionGoal()),
)
rospy.loginfo("Robot %s", ('Enabled' if status else 'Disabled'))
def state(self):
"""
Returns the last known robot state.
:rtype: str
:return: "Enabled"/"Disabled"
"""
return "%sabled"%('en' if self._enabled else 'dis',)
def enable(self):
"""
Enable all joints
"""
if not self._enabled:
rospy.loginfo("Robot Stopped: Attempting Reset...")
self._toggle_enabled(True)
def disable(self):
"""
Disable all joints
"""
self._toggle_enabled(False)
================================================
FILE: franka_interface/src/franka_interface/robot_params.py
================================================
# /***************************************************************************
#
# @package: franka_interface
# @metapackage: franka_ros_interface
# @author: Saif Sidhik <sxs1412@bham.ac.uk>
#
# **************************************************************************/
# /***************************************************************************
# Copyright (c) 2019-2021, Saif Sidhik
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
# http://www.apache.org/licenses/LICENSE-2.0
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
# **************************************************************************/
"""
:info:
Interface class for collecting and storing necessary parameters from ROS parameter server.
"""
import sys
import rospy
import socket
from franka_core_msgs.msg import JointLimits
def _log_networking_error():
print ("Failed to connect to the ROS parameter server!\n"
"Please check to make sure your ROS networking is "
"properly configured:\n")
sys.exit()
class RobotParams(object):
"""
Interface class for essential ROS parameters on the Franka Emika Panda robot.
"""
def __init__(self):
if self._robot_in_simulation():
self._in_sim = True
self._ns = "/panda_simulator"
rospy.loginfo("Robot control running in Simulation Mode!")
self._robot_ip="sim"
else:
self._ns = "/franka_ros_interface"
self._in_sim = False
self._robot_ip = self.get_robot_ip()
self._robot_name = self.get_robot_name()
def get_base_namespace(self):
return self._ns
def _robot_in_simulation(self):
sim = False
try:
sim = rospy.get_param("/SIMULATOR_")
except KeyError:
sim = False
except (socket.error, socket.gaierror):
_log_networking_error()
return sim
def get_neutral_pose(self):
"""
Get neutral pose joint positions from parameter server
(/robot_config/neutral_pose)
:return: Joint positions of the robot as defined in parameter server.
:rtype: [type]
"""
try:
neutral_pose = rospy.get_param("/robot_config/neutral_pose")
except KeyError:
rospy.logerr("RobotParam: robot_ip cannot detect neutral joint pos."
" under param /franka_control/neutral_pose")
except (socket.error, socket.gaierror):
_log_networking_error()
return neutral_pose
def get_robot_ip(self):
robot_ip = None
try:
robot_ip = rospy.get_param("/franka_control/robot_ip")
except KeyError:
try:
robot_ip = rospy.get_param("/robot_config/robot_ip")
except KeyError:
rospy.logerr("RobotParam: robot_ip cannot detect robot ip."
" under param /robot_ip")
except (socket.error, socket.gaierror):
_log_networking_error()
return robot_ip
def get_joint_names(self):
"""
Return the names of the joints for the robot from ROS parameter
server (/robot_config/joint_names).
:rtype: [str]
:return: ordered list of joint names from proximal to distal
(i.e. shoulder to wrist). joint names for limb
"""
joint_names = list()
try:
joint_names = rospy.get_param("/robot_config/joint_names")
except KeyError:
rospy.logerr("RobotParam: get_joint_names cannot detect joint_names for arm")
except (socket.error, socket.gaierror):
_log_networking_error()
return joint_names
def get_gripper_joint_names(self):
"""
Return the names of the joints for the gripper from ROS
parameter server (/gripper_config/joint_names).
:rtype: [str]
:return: ordered list of joint names
"""
joint_names = list()
try:
joint_names = rospy.get_param("/gripper_config/joint_names")
except KeyError:
rospy.loginfo("RobotParam: get_gripper_joint_names cannot detect joint_names for gripper.")
return None
except (socket.error, socket.gaierror):
_log_networking_error()
return joint_names
def get_robot_name(self):
"""
Return the name of class of robot from ROS parameter server.
(/robot_config/arm_id)
:rtype: str
:return: name of the robot
"""
robot_name = None
try:
robot_name = rospy.get_param("/robot_config/arm_id")
except KeyError:
rospy.logerr("RobotParam: get_robot_name cannot detect robot name"
" under param /robot_config/arm_id")
except (socket.error, socket.gaierror):
_log_networking_error()
return robot_name
def get_joint_limits(self):
"""
Get joint limits as defined in ROS parameter server
(/robot_config/joint_config/joint_velocity_limit)
:return: Joint limits for each joints
:rtype: franka_core_msgs.msg.JointLimits
"""
lims = JointLimits()
lims.joint_names = self.get_joint_names()
try:
vel_lim = rospy.get_param("/robot_config/joint_config/joint_velocity_limit")
except KeyError:
rospy.logerr("RobotParam: get_joint_limits cannot detect robot joint velocity limits"
" under param /robot_config/joint_config/joint_velocity_limit")
except (socket.error, socket.gaierror):
_log_networking_error()
try:
pos_min_lim = rospy.get_param("/robot_config/joint_config/joint_position_limit/lower")
except KeyError:
rospy.logerr("RobotParam: get_joint_limits cannot detect robot joint position lower limits"
" under param /robot_config/joint_config/joint_position_limit/lower")
except (socket.error, socket.gaierror):
_log_networking_error()
try:
pos_max_lim = rospy.get_param("/robot_config/joint_config/joint_position_limit/upper")
except KeyError:
rospy.logerr("RobotParam: get_joint_limits cannot detect robot joint position upper limits"
" under param /robot_config/joint_config/joint_position_limit/upper")
except (socket.error, socket.gaierror):
_log_networking_error()
try:
eff_lim = rospy.get_param("/robot_config/joint_config/joint_effort_limit")
except KeyError:
rospy.logerr("RobotParam: get_joint_limits cannot detect robot joint torque limits"
" under param /robot_config/joint_config/joint_effort_limit")
except (socket.error, socket.gaierror):
_log_networking_error()
try:
acc_lim = rospy.get_param("/robot_config/joint_config/joint_acceleration_limit")
except KeyError:
rospy.logerr("RobotParam: get_joint_limits cannot detect robot joint acceleration limits"
" under param /robot_config/joint_config/joint_acceleration_limit")
except (socket.error, socket.gaierror):
_log_networking_error()
for i in range(len(lims.joint_names)):
lims.position_upper.append(pos_max_lim[lims.joint_names[i]])
lims.position_lower.append(pos_min_lim[lims.joint_names[i]])
lims.velocity.append(vel_lim[lims.joint_names[i]])
lims.accel.append(acc_lim[lims.joint_names[i]])
lims.effort.append(eff_lim[lims.joint_names[i]])
return lims
if __name__ == '__main__':
rp = RobotParams()
# print rp.__dict__
print(rp.get_robot_ip())
print(rp.get_robot_name())
print(rp.get_joint_names())
================================================
FILE: franka_interface/src/motion_controller_interface.cpp
================================================
/***************************************************************************
* Adapted from arm_controller_interface.cpp (sawyer_simulator package)
*
* @package: franka_interface
* @metapackage: franka_ros_interface
* @author: Saif Sidhik <sxs1412@bham.ac.uk>
*
**************************************************************************/
/***************************************************************************
* Copyright (c) 2019-2021, Saif Sidhik
* Copyright (c) 2013-2018, Rethink Robotics Inc.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
**************************************************************************/
#include <franka_interface/motion_controller_interface.h>
#include <controller_manager_msgs/SwitchController.h>
namespace franka_interface {
void MotionControllerInterface::init(ros::NodeHandle& nh,
boost::shared_ptr<controller_manager::ControllerManager> controller_manager) {
current_mode_ = -1;
if (!nh.getParam("/controllers_config/position_controller", position_controller_name_)) {
position_controller_name_ = "position_joint_position_controller";
}
if (!nh.getParam("/controllers_config/torque_controller", torque_controller_name_)) {
torque_controller_name_ = "effort_joint_torque_controller";
}
if (!nh.getParam("/controllers_config/impedance_controller", impedance_controller_name_)) {
impedance_controller_name_ = "effort_joint_impedance_controller";
}
if (!nh.getParam("/controllers_config/velocity_controller", velocity_controller_name_)) {
velocity_controller_name_ = "velocity_joint_velocity_controller";
}
if (!nh.getParam("/controllers_config/trajectory_controller", trajectory_controller_name_)) {
trajectory_controller_name_ = "position_joint_trajectory_controller";
}
if (!nh.getParam("/controllers_config/default_controller", default_controller_name_)) {
default_controller_name_ = "position_joint_trajectory_controller";
}
current_controller_name_ = default_controller_name_;
all_controllers_.clear();
all_controllers_.push_back(position_controller_name_);
all_controllers_.push_back(torque_controller_name_);
all_controllers_.push_back(impedance_controller_name_);
all_controllers_.push_back(velocity_controller_name_);
all_controllers_.push_back(trajectory_controller_name_);
bool default_defined = false;
for (size_t i = 0; i < all_controllers_.size(); ++i){
if (all_controllers_[i] == default_controller_name_){
default_defined = true;
break;
}
}
controller_name_to_mode_map_[position_controller_name_] = franka_core_msgs::JointCommand::POSITION_MODE;
controller_name_to_mode_map_[torque_controller_name_] = franka_core_msgs::JointCommand::TORQUE_MODE;
controller_name_to_mode_map_[impedance_controller_name_] = franka_core_msgs::JointCommand::IMPEDANCE_MODE;
controller_name_to_mode_map_[velocity_controller_name_] = franka_core_msgs::JointCommand::VELOCITY_MODE;
controller_name_to_mode_map_[trajectory_controller_name_] = -1;
if (! default_defined){
ROS_ERROR_STREAM_NAMED("MotionControllerInterface", "Default controller not present in the provided controllers!");
}
controller_manager_ = controller_manager;
joint_command_sub_ = nh.subscribe("/franka_ros_interface/motion_controller/arm/joint_commands", 1,
&MotionControllerInterface::jointCommandCallback, this);
// Command Timeout
joint_command_timeout_sub_ = nh.subscribe("/franka_ros_interface/motion_controller/arm/joint_command_timeout", 1,
&MotionControllerInterface::jointCommandTimeoutCallback, this);
double command_timeout_default;
nh.param<double>("/controllers_config/command_timeout", command_timeout_default, 0.2);
auto p_cmd_timeout_length = std::make_shared<ros::Duration>(std::min(1.0,
std::max(0.0, command_timeout_default)));
box_timeout_length_.set(p_cmd_timeout_length);
ROS_INFO_STREAM("MotionControllerInterface Initialised");
// Update at 100Hz
cmd_timeout_timer_ = nh.createTimer(100, &MotionControllerInterface::commandTimeoutCheck, this);
}
void MotionControllerInterface::commandTimeoutCheck(const ros::TimerEvent& e) {
// lock out other thread(s) which are getting called back via ros.
std::lock_guard<std::mutex> guard(mtx_);
// Check Command Timeout
std::shared_ptr<const ros::Duration> p_timeout_length;
box_timeout_length_.get(p_timeout_length);
std::shared_ptr<const ros::Time> p_cmd_msg_time;
box_cmd_timeout_.get(p_cmd_msg_time);
bool command_timeout = (p_cmd_msg_time && p_timeout_length &&
((ros::Time::now() - *p_cmd_msg_time.get()) > (*p_timeout_length.get())));
if(command_timeout && (current_controller_name_ != default_controller_name_)) {
// Timeout violated, force robot back to Default Controller Mode
ROS_WARN_STREAM("MotionControllerInterface: Command timeout violated: Switching to Default control mode." << default_controller_name_);
switchToDefaultController();
}
}
bool MotionControllerInterface::switchToDefaultController() {
std::vector<std::string> start_controllers;
std::vector<std::string> stop_controllers;
for (size_t i = 0; i < all_controllers_.size(); ++i) {
if (all_controllers_[i] == default_controller_name_)
start_controllers.push_back(all_controllers_[i]);
else stop_controllers.push_back(all_controllers_[i]);
}
if (!controller_manager_->switchController(start_controllers, stop_controllers,
controller_manager_msgs::SwitchController::Request::BEST_EFFORT))
{
ROS_ERROR_STREAM_NAMED("MotionControllerInterface", "Failed to switch controllers");
return false;
}
current_controller_name_ = start_controllers[0];
current_mode_ = controller_name_to_mode_map_[current_controller_name_];
ROS_INFO_STREAM("MotionControllerInterface: Controller " << start_controllers[0]
<< " started; Controllers " << stop_controllers[0] <<
", " << stop_controllers[1] <<
", " << stop_controllers[2] <<
", " << stop_controllers[3] << " stopped.");
return true;
}
void MotionControllerInterface::jointCommandTimeoutCallback(const std_msgs::Float64 msg) {
ROS_INFO_STREAM("MotionControllerInterface: Joint command timeout: " << msg.data);
auto p_cmd_timeout_length = std::make_shared<ros::Duration>(
std::min(1.0, std::max(0.0, double(msg.data))));
box_timeout_length_.set(p_cmd_timeout_length);
}
bool MotionControllerInterface::switchControllers(int control_mode) {
std::vector<std::string> start_controllers;
std::vector<std::string> stop_controllers;
if(current_mode_ != control_mode)
{
switch (control_mode)
{
case franka_core_msgs::JointCommand::POSITION_MODE:
start_controllers.push_back(position_controller_name_);
stop_controllers.push_back(impedance_controller_name_);
stop_controllers.push_back(torque_controller_name_);
stop_controllers.push_back(velocity_controller_name_);
stop_controllers.push_back(trajectory_controller_name_);
break;
case franka_core_msgs::JointCommand::IMPEDANCE_MODE:
start_controllers.push_back(impedance_controller_name_);
stop_controllers.push_back(position_controller_name_);
stop_controllers.push_back(torque_controller_name_);
stop_controllers.push_back(velocity_controller_name_);
stop_controllers.push_back(trajectory_controller_name_);
break;
case franka_core_msgs::JointCommand::TORQUE_MODE:
start_controllers.push_back(torque_controller_name_);
stop_controllers.push_back(position_controller_name_);
stop_controllers.push_back(impedance_controller_name_);
stop_controllers.push_back(velocity_controller_name_);
stop_controllers.push_back(trajectory_controller_name_);
break;
case franka_core_msgs::JointCommand::VELOCITY_MODE:
start_controllers.push_back(velocity_controller_name_);
stop_controllers.push_back(position_controller_name_);
stop_controllers.push_back(impedance_controller_name_);
stop_controllers.push_back(torque_controller_name_);
stop_controllers.push_back(trajectory_controller_name_);
break;
default:
ROS_ERROR_STREAM_NAMED("MotionControllerInterface", "Unknown JointCommand mode "
<< control_mode << ". Ignoring command.");
return false;
}
if (!controller_manager_->switchController(start_controllers, stop_controllers,
controller_manager_msgs::SwitchController::Request::BEST_EFFORT))
{
ROS_ERROR_STREAM_NAMED("MotionControllerInterface", "Failed to switch controllers");
return false;
}
current_mode_ = control_mode;
current_controller_name_ = start_controllers[0];
ROS_INFO_STREAM("MotionControllerInterface: Controller " << start_controllers[0]
<< " started; Controllers " << stop_controllers[0] <<
", " << stop_controllers[1] <<
", " << stop_controllers[2] <<
", " << stop_controllers[3] << " stopped.");
}
return true;
}
void MotionControllerInterface::jointCommandCallback(const franka_core_msgs::JointCommandConstPtr& msg) {
// lock out other thread(s) which are getting called back via ros.
std::lock_guard<std::mutex> guard(mtx_);
if(switchControllers(msg->mode)) {
auto p_cmd_msg_time = std::make_shared<ros::Time>(ros::Time::now());
box_cmd_timeout_.set(p_cmd_msg_time);
}
}
}
================================================
FILE: franka_interface/src/robot_state_controller.cpp
================================================
/***************************************************************************
* Adapted from robot_state_publisher.cpp (franka_ros package)
*
* @package: panda_gazebo
* @metapackage: panda_simulator
* @author: Saif Sidhik <sxs1412@bham.ac.uk>
*
**************************************************************************/
/***************************************************************************
* Copyright (c) 2019-2021, Saif Sidhik.
* Copyright (c) 2017 Franka Emika GmbH
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
**************************************************************************/
#include <franka_interface/robot_state_controller.h>
#include <cmath>
#include <memory>
#include <mutex>
#include <string>
#include <franka/errors.h>
#include <franka_hw/franka_cartesian_command_interface.h>
#include <franka_msgs/Errors.h>
#include <hardware_interface/hardware_interface.h>
#include <pluginlib/class_list_macros.h>
#include <ros/ros.h>
#include <tf/tf.h>
#include <tf/transform_datatypes.h>
namespace {
tf::Transform convertArrayToTf(const std::array<double, 16>& transform) {
tf::Matrix3x3 rotation(transform[0], transform[4], transform[8], transform[1], transform[5],
transform[9], transform[2], transform[6], transform[10]);
tf::Vector3 translation(transform[12], transform[13], transform[14]);
return tf::Transform(rotation, translation);
}
franka_msgs::Errors errorsToMessage(const franka::Errors& error) {
franka_msgs::Errors message;
message.joint_position_limits_violation =
static_cast<decltype(message.joint_position_limits_violation)>(
error.joint_position_limits_violation);
message.cartesian_position_limits_violation =
static_cast<decltype(message.cartesian_position_limits_violation)>(
error.cartesian_position_limits_violation);
message.self_collision_avoidance_violation =
static_cast<decltype(message.self_collision_avoidance_violation)>(
error.self_collision_avoidance_violation);
message.joint_velocity_violation =
static_cast<decltype(message.joint_velocity_violation)>(error.joint_velocity_violation);
message.cartesian_velocity_violation =
static_cast<decltype(message.cartesian_velocity_violation)>(
error.cartesian_velocity_violation);
message.force_control_safety_violation =
static_cast<decltype(message.force_control_safety_violation)>(
error.force_control_safety_violation);
message.joint_reflex = static_cast<decltype(message.joint_reflex)>(error.joint_reflex);
message.cartesian_reflex =
static_cast<decltype(message.cartesian_reflex)>(error.cartesian_reflex);
message.max_goal_pose_deviation_violation =
static_cast<decltype(message.max_goal_pose_deviation_violation)>(
error.max_goal_pose_deviation_violation);
message.max_path_pose_deviation_violation =
static_cast<decltype(message.max_path_pose_deviation_violation)>(
error.max_path_pose_deviation_violation);
message.cartesian_velocity_profile_safety_violation =
static_cast<decltype(message.cartesian_velocity_profile_safety_violation)>(
error.cartesian_velocity_profile_safety_violation);
message.joint_position_motion_generator_start_pose_invalid =
static_cast<decltype(message.joint_position_motion_generator_start_pose_invalid)>(
error.joint_position_motion_generator_start_pose_invalid);
message.joint_motion_generator_position_limits_violation =
static_cast<decltype(message.joint_motion_generator_position_limits_violation)>(
error.joint_motion_generator_position_limits_violation);
message.joint_motion_generator_velocity_limits_violation =
static_cast<decltype(message.joint_motion_generator_velocity_limits_violation)>(
error.joint_motion_generator_velocity_limits_violation);
message.joint_motion_generator_velocity_discontinuity =
static_cast<decltype(message.joint_motion_generator_velocity_discontinuity)>(
error.joint_motion_generator_velocity_discontinuity);
message.joint_motion_generator_acceleration_discontinuity =
static_cast<decltype(message.joint_motion_generator_acceleration_discontinuity)>(
error.joint_motion_generator_acceleration_discontinuity);
message.cartesian_position_motion_generator_start_pose_invalid =
static_cast<decltype(message.cartesian_position_motion_generator_start_pose_invalid)>(
error.cartesian_position_motion_generator_start_pose_invalid);
message.cartesian_motion_generator_elbow_limit_violation =
static_cast<decltype(message.cartesian_motion_generator_elbow_limit_violation)>(
error.cartesian_motion_generator_elbow_limit_violation);
message.cartesian_motion_generator_velocity_limits_violation =
static_cast<decltype(message.cartesian_motion_generator_velocity_limits_violation)>(
error.cartesian_motion_generator_velocity_limits_violation);
message.cartesian_motion_generator_velocity_discontinuity =
static_cast<decltype(message.cartesian_motion_generator_velocity_discontinuity)>(
error.cartesian_motion_generator_velocity_discontinuity);
message.cartesian_motion_generator_acceleration_discontinuity =
static_cast<decltype(message.cartesian_motion_generator_acceleration_discontinuity)>(
error.cartesian_motion_generator_acceleration_discontinuity);
message.cartesian_motion_generator_elbow_sign_inconsistent =
static_cast<decltype(message.cartesian_motion_generator_elbow_sign_inconsistent)>(
error.cartesian_motion_generator_elbow_sign_inconsistent);
message.cartesian_motion_generator_start_elbow_invalid =
static_cast<decltype(message.cartesian_motion_generator_start_elbow_invalid)>(
error.cartesian_motion_generator_start_elbow_invalid);
message.cartesian_motion_generator_joint_position_limits_violation =
static_cast<decltype(message.cartesian_motion_generator_joint_position_limits_violation)>(
error.cartesian_motion_generator_joint_position_limits_violation);
message.cartesian_motion_generator_joint_velocity_limits_violation =
static_cast<decltype(message.cartesian_motion_generator_joint_velocity_limits_violation)>(
error.cartesian_motion_generator_joint_velocity_limits_violation);
message.cartesian_motion_generator_joint_velocity_discontinuity =
static_cast<decltype(message.cartesian_motion_generator_joint_velocity_discontinuity)>(
error.cartesian_motion_generator_joint_velocity_discontinuity);
message.cartesian_motion_generator_joint_acceleration_discontinuity =
static_cast<decltype(message.cartesian_motion_generator_joint_acceleration_discontinuity)>(
error.cartesian_motion_generator_joint_acceleration_discontinuity);
message.cartesian_position_motion_generator_invalid_frame =
static_cast<decltype(message.cartesian_position_motion_generator_invalid_frame)>(
error.cartesian_position_motion_generator_invalid_frame);
message.force_controller_desired_force_tolerance_violation =
static_cast<decltype(message.force_controller_desired_force_tolerance_violation)>(
error.force_controller_desired_force_tolerance_violation);
message.controller_torque_discontinuity =
static_cast<decltype(message.controller_torque_discontinuity)>(
error.controller_torque_discontinuity);
message.start_elbow_sign_inconsistent =
static_cast<decltype(message.start_elbow_sign_inconsistent)>(
error.start_elbow_sign_inconsistent);
message.communication_constraints_violation =
static_cast<decltype(message.communication_constraints_violation)>(
error.communication_constraints_violation);
message.power_limit_violation =
static_cast<decltype(message.power_limit_violation)>(error.power_limit_violation);
message.joint_p2p_insufficient_torque_for_planning =
static_cast<decltype(message.joint_p2p_insufficient_torque_for_planning)>(
error.joint_p2p_insufficient_torque_for_planning);
message.tau_j_range_violation =
static_cast<decltype(message.tau_j_range_violation)>(error.tau_j_range_violation);
message.instability_detected =
static_cast<decltype(message.instability_detected)>(error.instability_detected);
return message;
}
} // anonymous namespace
namespace franka_interface {
bool CustomFrankaStateController::init(hardware_interface::RobotHW* robot_hardware,
ros::NodeHandle& root_node_handle,
ros::NodeHandle& controller_node_handle) {
franka_state_interface_ = robot_hardware->get<franka_hw::FrankaStateInterface>();
if (franka_state_interface_ == nullptr) {
ROS_ERROR("CustomFrankaStateController: Could not get Franka state interface from hardware");
return false;
}
if (!controller_node_handle.getParam("arm_id", arm_id_)) {
ROS_ERROR("CustomFrankaStateController: Could not get parameter arm_id");
return false;
}
double publish_rate(500.0);
if (!controller_node_handle.getParam("publish_rate", publish_rate)) {
ROS_INFO_STREAM("CustomFrankaStateController: Did not find publish_rate. Using default "
<< publish_rate << " [Hz].");
}
trigger_publish_ = franka_hw::TriggerRate(publish_rate);
if (!controller_node_handle.getParam("joint_names", joint_names_) ||
joint_names_.size() != robot_state_.q.size()) {
ROS_ERROR(
"CustomFrankaStateController: Invalid or no joint_names parameters provided, aborting "
"controller init!");
return false;
}
try {
franka_state_handle_ = std::make_unique<franka_hw::FrankaStateHandle>(
franka_state_interface_->getHandle(arm_id_ + "_robot"));
} catch (const hardware_interface::HardwareInterfaceException& ex) {
ROS_ERROR_STREAM("CustomFrankaStateController: Exception getting franka state handle: " << ex.what());
return false;
}
auto* model_interface = robot_hardware->get<franka_hw::FrankaModelInterface>();
if (model_interface == nullptr) {
ROS_ERROR_STREAM(
"CustomFrankaStateController: Error getting model interface from hardware");
return false;
}
try {
model_handle_ = std::make_unique<franka_hw::FrankaModelHandle>(
model_interface->getHandle(arm_id_ + "_model"));
} catch (hardware_interface::HardwareInterfaceException& ex) {
ROS_ERROR_STREAM(
"CustomFrankaStateController: Exception getting model handle from interface: "
<< ex.what());
return false;
}
publisher_transforms_.init(root_node_handle, "/tf", 1);
publisher_franka_state_.init(controller_node_handle, "robot_state", 1);
publisher_joint_states_.init(controller_node_handle, "joint_states", 1);
publisher_joint_states_desired_.init(controller_node_handle, "joint_states_desired", 1);
publisher_tip_state_.init(controller_node_handle, "tip_state", 1);
{
std::lock_guard<realtime_tools::RealtimePublisher<sensor_msgs::JointState> > lock(
publisher_joint_states_);
publisher_joint_states_.msg_.name.resize(joint_names_.size());
publisher_joint_states_.msg_.position.resize(robot_state_.q.size());
publisher_joint_states_.msg_.velocity.resize(robot_state_.dq.size());
publisher_joint_states_.msg_.effort.resize(robot_state_.tau_J.size());
}
{
std::lock_guard<realtime_tools::RealtimePublisher<sensor_msgs::JointState> > lock(
publisher_joint_states_desired_);
publisher_joint_states_desired_.msg_.name.resize(joint_names_.size());
publisher_joint_states_desired_.msg_.position.resize(robot_state_.q_d.size());
publisher_joint_states_desired_.msg_.velocity.resize(robot_state_.dq_d.size());
publisher_joint_states_desired_.msg_.effort.resize(robot_state_.tau_J_d.size());
}
{
std::lock_guard<realtime_tools::RealtimePublisher<tf2_msgs::TFMessage> > lock(
publisher_transforms_);
publisher_transforms_.msg_.transforms.resize(3);
tf::Quaternion quaternion(0.0, 0.0, 0.0, 1.0);
tf::Vector3 translation(0.0, 0.0, 0.0);
tf::Transform transform(quaternion, translation);
tf::StampedTransform stamped_transform(transform, ros::Time::now(), arm_id_ + "_link8",
arm_id_ + "_NE");
geometry_msgs::TransformStamped transform_message;
transformStampedTFToMsg(stamped_transform, transform_message);
publisher_transforms_.msg_.transforms[0] = transform_message;
translation = tf::Vector3(0.0, 0.0, 0.0);
transform = tf::Transform(quaternion, translation);
stamped_transform =
tf::StampedTransform(transform, ros::Time::now(), arm_id_ + "_NE", arm_id_ + "_EE");
transformStampedTFToMsg(stamped_transform, transform_message);
publisher_transforms_.msg_.transforms[1] = transform_message;
translation = tf::Vector3(0.0, 0.0, 0.0);
transform = tf::Transform(quaternion, translation);
stamped_transform =
tf::StampedTransform(transform, ros::Time::now(), arm_id_ + "_EE", arm_id_ + "_K");
transformStampedTFToMsg(stamped_transform, transform_message);
publisher_transforms_.msg_.transforms[2] = transform_message;
}
{
std::lock_guard<realtime_tools::RealtimePublisher<franka_core_msgs::EndPointState> > lock(
publisher_tip_state_);
publisher_tip_state_.msg_.O_F_ext_hat_K.header.frame_id = arm_id_ + "_link0";
publisher_tip_state_.msg_.O_F_ext_hat_K.wrench.force.x = 0.0;
publisher_tip_state_.msg_.O_F_ext_hat_K.wrench.force.y = 0.0;
publisher_tip_state_.msg_.O_F_ext_hat_K.wrench.force.z = 0.0;
publisher_tip_state_.msg_.O_F_ext_hat_K.wrench.torque.x = 0.0;
publisher_tip_state_.msg_.O_F_ext_hat_K.wrench.torque.y = 0.0;
publisher_tip_state_.msg_.O_F_ext_hat_K.wrench.torque.z = 0.0;
publisher_tip_state_.msg_.K_F_ext_hat_K.header.frame_id = arm_id_ + "_K";
publisher_tip_state_.msg_.K_F_ext_hat_K.wrench.force.x = 0.0;
publisher_tip_state_.msg_.K_F_ext_hat_K.wrench.force.y = 0.0;
publisher_tip_state_.msg_.K_F_ext_hat_K.wrench.force.z = 0.0;
publisher_tip_state_.msg_.K_F_ext_hat_K.wrench.torque.x = 0.0;
publisher_tip_state_.msg_.K_F_ext_hat_K.wrench.torque.y = 0.0;
publisher_tip_state_.msg_.K_F_ext_hat_K.wrench.torque.z = 0.0;
}
return true;
}
void CustomFrankaStateController::update(const ros::Time& time, const ros::Duration& /* period */) {
if (trigger_publish_()) {
robot_state_ = franka_state_handle_->getRobotState();
publishFrankaState(time);
publishTransforms(time);
publishEndPointState(time);
publishJointStates(time);
sequence_number_++;
}
}
void CustomFrankaStateController::publishFrankaState(const ros::Time& time) {
std::array<double, 7> coriolis = model_handle_->getCoriolis();
std::array<double, 7> gravity = model_handle_->getGravity();
std::array<double, 49> mass_matrix = model_handle_->getMass();
std::array<double, 42> O_Jac_EE = model_handle_->getZeroJacobian(franka::Frame::kEndEffector);
Eigen::Map<Eigen::Matrix<double, 6, 7>> jacobian(O_Jac_EE.data());
Eigen::Map<Eigen::Matrix<double, 7, 1>> dq(robot_state_.dq.data());
// jacobian * dq
Eigen::Matrix<double, 6, 1> ee_vel = jacobian * dq;
if (publisher_franka_state_.trylock()) {
static_assert(
sizeof(robot_state_.cartesian_collision) == sizeof(robot_state_.cartesian_contact),
"Robot state Cartesian members do not have same size");
static_assert(
sizeof(robot_state_.cartesian_collision) == sizeof(robot_state_.O_dP_EE_c),
"Robot state Cartesian members do not have same size");
static_assert(
sizeof(robot_state_.cartesian_collision) == sizeof(robot_state_.O_dP_EE_d),
"Robot state Cartesian members do not have same size");
static_assert(
sizeof(robot_state_.cartesian_collision) == sizeof(robot_state_.O_ddP_EE_c),
"Robot state Cartesian members do not have same size");
for (size_t i = 0; i < robot_state_.cartesian_collision.size(); i++) {
publisher_franka_state_.msg_.cartesian_collision[i] = robot_state_.cartesian_collision[i];
publisher_franka_state_.msg_.cartesian_contact[i] = robot_state_.cartesian_contact[i];
publisher_franka_state_.msg_.O_dP_EE[i] = ee_vel(i,0);
}
static_assert(sizeof(robot_state_.q) == sizeof(robot_state_.q_d),
"Robot state joint members do not have same size");
static_assert(sizeof(robot_state_.q) == sizeof(robot_state_.dq_d),
"Robot state joint members do not have same size");
static_assert(sizeof(robot_state_.q) == sizeof(robot_state_.dtau_J),
"Robot state joint members do not have same size");
static_assert(sizeof(robot_state_.q) == sizeof(robot_state_.tau_J_d),
"Robot state joint members do not have same size");
static_assert(sizeof(robot_state_.q) == sizeof(robot_state_.joint_collision),
"Robot state joint members do not have same size");
static_assert(sizeof(robot_state_.q) == sizeof(robot_state_.joint_contact),
"Robot state joint members do not have same size");
static_assert(sizeof(robot_state_.q) == sizeof(robot_state_.tau_ext_hat_filtered),
"Robot state joint members do not have same size");
for (size_t i = 0; i < robot_state_.q.size(); i++) {
publisher_franka_state_.msg_.q_d[i] = robot_state_.q_d[i];
publisher_franka_state_.msg_.dq_d[i] = robot_state_.dq_d[i];
publisher_franka_state_.msg_.dtau_J[i] = robot_state_.dtau_J[i];
publisher_franka_state_.msg_.tau_J_d[i] = robot_state_.tau_J_d[i];
publisher_franka_state_.msg_.joint_collision[i] = robot_state_.joint_collision[i];
publisher_franka_state_.msg_.joint_contact[i] = robot_state_.joint_contact[i];
publisher_franka_state_.msg_.tau_ext_hat_filtered[i] = robot_state_.tau_ext_hat_filtered[i];
publisher_franka_state_.msg_.gravity[i] = gravity[i];
publisher_franka_state_.msg_.coriolis[i] = coriolis[i];
}
static_assert(sizeof(robot_state_.O_T_EE) == sizeof(robot_state_.F_T_EE),
"Robot state transforms do not have same size");
static_assert(sizeof(robot_state_.O_T_EE) == sizeof(robot_state_.EE_T_K),
"Robot state transforms do not have same size");
static_assert(sizeof(robot_state_.O_T_EE) == sizeof(robot_state_.O_T_EE_d),
"Robot state transforms do not have same size");
static_assert(sizeof(robot_state_.NE_T_EE) == sizeof(robot_state_.O_T_EE),
"Robot state transforms do not have sa
gitextract_jpas59zs/
├── .github/
│ └── workflows/
│ └── auto-comment.yml
├── .gitignore
├── .travis.yml
├── CMakeLists.txt
├── LICENSE
├── NOTICE
├── README.md
├── cmake/
│ └── ClangTools.cmake
├── franka.sh
├── franka_common/
│ └── franka_core_msgs/
│ ├── CMakeLists.txt
│ ├── msg/
│ │ ├── EndPointState.msg
│ │ ├── JointCommand.msg
│ │ ├── JointControllerStates.msg
│ │ ├── JointLimits.msg
│ │ └── RobotState.msg
│ └── package.xml
├── franka_interface/
│ ├── CMakeLists.txt
│ ├── README.md
│ ├── config/
│ │ ├── basic_controllers.yaml
│ │ └── robot_config.yaml
│ ├── include/
│ │ └── franka_interface/
│ │ ├── motion_controller_interface.h
│ │ └── robot_state_controller.h
│ ├── launch/
│ │ └── interface.launch
│ ├── package.xml
│ ├── scripts/
│ │ ├── enable_robot.py
│ │ ├── move_to_neutral.py
│ │ └── simple_gripper.py
│ ├── setup.py
│ ├── src/
│ │ ├── franka_control_node.cpp
│ │ ├── franka_dataflow/
│ │ │ ├── __init__.py
│ │ │ ├── getch.py
│ │ │ └── wait_for.py
│ │ ├── franka_interface/
│ │ │ ├── __init__.py
│ │ │ ├── arm.py
│ │ │ ├── gripper.py
│ │ │ ├── robot_enable.py
│ │ │ └── robot_params.py
│ │ ├── motion_controller_interface.cpp
│ │ └── robot_state_controller.cpp
│ ├── state_controller_plugin.xml
│ └── tests/
│ ├── demo_interface.py
│ ├── demo_joint_positions_keyboard.py
│ └── test_zeroG.py
├── franka_moveit/
│ ├── CMakeLists.txt
│ ├── README.md
│ ├── config/
│ │ └── moveit_demo.rviz
│ ├── launch/
│ │ └── demo_moveit.launch
│ ├── package.xml
│ ├── scripts/
│ │ └── create_demo_planning_scene.py
│ ├── setup.py
│ ├── src/
│ │ └── franka_moveit/
│ │ ├── __init__.py
│ │ ├── extended_planning_scene_interface.py
│ │ ├── movegroup_interface.py
│ │ └── utils.py
│ └── tests/
│ ├── test_movegroup_interface.py
│ └── test_scene_interface.py
├── franka_ros_controllers/
│ ├── CMakeLists.txt
│ ├── README.md
│ ├── cfg/
│ │ └── joint_controller_params.cfg
│ ├── config/
│ │ └── ros_controllers.yaml
│ ├── controller_plugins.xml
│ ├── include/
│ │ └── franka_ros_controllers/
│ │ ├── effort_joint_impedance_controller.h
│ │ ├── effort_joint_position_controller.h
│ │ ├── effort_joint_torque_controller.h
│ │ ├── position_joint_position_controller.h
│ │ └── velocity_joint_velocity_controller.h
│ ├── package.xml
│ ├── scripts/
│ │ └── test_controller.py
│ └── src/
│ ├── effort_joint_impedance_controller.cpp
│ ├── effort_joint_position_controller.cpp
│ ├── effort_joint_torque_controller.cpp
│ ├── position_joint_position_controller.cpp
│ └── velocity_joint_velocity_controller.cpp
├── franka_ros_interface/
│ ├── CMakeLists.txt
│ └── package.xml
└── franka_tools/
├── CMakeLists.txt
├── README.md
├── package.xml
├── setup.py
└── src/
└── franka_tools/
├── __init__.py
├── collision_behaviour_interface.py
├── controller_manager_interface.py
├── controller_param_config_client.py
├── frames_interface.py
└── joint_trajectory_action_client.py
SYMBOL INDEX (233 symbols across 34 files)
FILE: franka_interface/include/franka_interface/motion_controller_interface.h
function namespace (line 41) | namespace franka_interface {
FILE: franka_interface/include/franka_interface/robot_state_controller.h
function namespace (line 49) | namespace franka_interface {
FILE: franka_interface/scripts/enable_robot.py
function main (line 52) | def main():
FILE: franka_interface/scripts/simple_gripper.py
function _active_cb (line 53) | def _active_cb():
function _feedback_cb (line 56) | def _feedback_cb(msg):
function _done_cb (line 59) | def _done_cb(status, result):
FILE: franka_interface/src/franka_control_node.cpp
function main (line 45) | int main(int argc, char** argv) {
FILE: franka_interface/src/franka_dataflow/getch.py
function getch (line 21) | def getch(timeout=0.01):
FILE: franka_interface/src/franka_dataflow/wait_for.py
function wait_for (line 34) | def wait_for(test, timeout=1.0, raise_on_error=True, rate=100,
FILE: franka_interface/src/franka_interface/arm.py
class TipState (line 57) | class TipState():
method __init__ (line 59) | def __init__(self, timestamp, pose, vel, O_effort, K_effort):
method pose (line 67) | def pose(self):
method velocity (line 71) | def velocity(self):
method effort (line 75) | def effort(self):
method effort_in_K_frame (line 79) | def effort_in_K_frame(self):
class ArmInterface (line 83) | class ArmInterface(object):
class RobotMode (line 105) | class RobotMode(enum.IntEnum):
method __init__ (line 121) | def __init__(self, synchronous_pub=False):
method _clean_shutdown (line 237) | def _clean_shutdown(self):
method get_movegroup_interface (line 244) | def get_movegroup_interface(self):
method get_robot_params (line 251) | def get_robot_params(self):
method get_joint_limits (line 258) | def get_joint_limits(self):
method joint_names (line 267) | def joint_names(self):
method _on_joint_states (line 276) | def _on_joint_states(self, msg):
method _on_robot_state (line 284) | def _on_robot_state(self, msg):
method coriolis_comp (line 323) | def coriolis_comp(self):
method gravity_comp (line 333) | def gravity_comp(self):
method get_robot_status (line 342) | def get_robot_status(self):
method in_safe_state (line 351) | def in_safe_state(self):
method error_in_current_state (line 360) | def error_in_current_state(self):
method what_errors (line 369) | def what_errors(self):
method _on_endpoint_state (line 378) | def _on_endpoint_state(self, msg):
method joint_angle (line 410) | def joint_angle(self, joint):
method joint_angles (line 421) | def joint_angles(self):
method joint_ordered_angles (line 430) | def joint_ordered_angles(self):
method joint_velocity (line 439) | def joint_velocity(self, joint):
method joint_velocities (line 450) | def joint_velocities(self):
method joint_effort (line 459) | def joint_effort(self, joint):
method joint_efforts (line 470) | def joint_efforts(self):
method endpoint_pose (line 479) | def endpoint_pose(self):
method endpoint_velocity (line 492) | def endpoint_velocity(self):
method endpoint_effort (line 504) | def endpoint_effort(self, in_base_frame=True):
method exit_control_mode (line 518) | def exit_control_mode(self, timeout=5, velocity_tolerance=1e-2):
method tip_states (line 560) | def tip_states(self):
method joint_inertia_matrix (line 569) | def joint_inertia_matrix(self):
method zero_jacobian (line 578) | def zero_jacobian(self):
method set_command_timeout (line 587) | def set_command_timeout(self, timeout):
method set_joint_position_speed (line 599) | def set_joint_position_speed(self, speed=0.3):
method set_joint_positions (line 621) | def set_joint_positions(self, positions):
method set_joint_velocities (line 634) | def set_joint_velocities(self, velocities):
method set_joint_torques (line 647) | def set_joint_torques(self, torques):
method set_joint_positions_velocities (line 660) | def set_joint_positions_velocities(self, positions, velocities):
method has_collided (line 680) | def has_collided(self):
method move_to_neutral (line 687) | def move_to_neutral(self, timeout=15.0, speed=0.15):
method move_to_joint_positions (line 701) | def move_to_joint_positions(self, positions, timeout=10.0,
method get_flange_pose (line 790) | def get_flange_pose(self, pos=None, ori=None):
method move_to_cartesian_pose (line 821) | def move_to_cartesian_pose(self, pos, ori=None, use_moveit=True):
method pause_controllers_and_do (line 861) | def pause_controllers_and_do(self, func, *args, **kwargs):
method reset_EE_frame (line 893) | def reset_EE_frame(self):
method set_EE_frame (line 918) | def set_EE_frame(self, frame):
method set_EE_at_frame (line 944) | def set_EE_at_frame(self, frame_name, timeout=5.0):
method set_collision_threshold (line 964) | def set_collision_threshold(self, cartesian_forces=None, joint_torques...
method get_controller_manager (line 980) | def get_controller_manager(self):
method get_frames_interface (line 987) | def get_frames_interface(self):
FILE: franka_interface/src/franka_interface/gripper.py
class GripperInterface (line 46) | class GripperInterface(object):
method __init__ (line 61) | def __init__(self, gripper_joint_names = ('panda_finger_joint1', 'pand...
method exists (line 126) | def exists(self):
method set_velocity (line 135) | def set_velocity(self, value):
method _joint_states_callback (line 147) | def _joint_states_callback(self, msg):
method joint_names (line 155) | def joint_names(self):
method joint_position (line 164) | def joint_position(self, joint):
method joint_positions (line 176) | def joint_positions(self):
method joint_ordered_positions (line 185) | def joint_ordered_positions(self):
method joint_velocity (line 194) | def joint_velocity(self, joint):
method joint_velocities (line 206) | def joint_velocities(self):
method joint_ordered_velocities (line 215) | def joint_ordered_velocities(self):
method joint_effort (line 225) | def joint_effort(self, joint):
method joint_efforts (line 237) | def joint_efforts(self):
method joint_ordered_efforts (line 246) | def joint_ordered_efforts(self):
method _active_cb (line 255) | def _active_cb(self):
method _feedback_cb (line 258) | def _feedback_cb(self, msg):
method _done_cb (line 261) | def _done_cb(self, status, result):
method home_joints (line 265) | def home_joints(self, wait_for_result = False):
method open (line 292) | def open(self):
method close (line 302) | def close(self):
method calibrate (line 326) | def calibrate(self):
method move_joints (line 329) | def move_joints(self, width, speed = None, wait_for_result = True):
method stop_action (line 362) | def stop_action(self):
method grasp (line 378) | def grasp(self, width, force, speed = None, epsilon_inner = 0.005, eps...
FILE: franka_interface/src/franka_interface/robot_enable.py
class RobotEnable (line 43) | class RobotEnable(object):
method __init__ (line 58) | def __init__(self, robot_params = None):
method _state_callback (line 85) | def _state_callback(self, msg):
method is_enabled (line 88) | def is_enabled(self):
method _toggle_enabled (line 98) | def _toggle_enabled(self, status):
method state (line 115) | def state(self):
method enable (line 124) | def enable(self):
method disable (line 132) | def disable(self):
FILE: franka_interface/src/franka_interface/robot_params.py
function _log_networking_error (line 38) | def _log_networking_error():
class RobotParams (line 44) | class RobotParams(object):
method __init__ (line 49) | def __init__(self):
method get_base_namespace (line 63) | def get_base_namespace(self):
method _robot_in_simulation (line 66) | def _robot_in_simulation(self):
method get_neutral_pose (line 77) | def get_neutral_pose(self):
method get_robot_ip (line 96) | def get_robot_ip(self):
method get_joint_names (line 111) | def get_joint_names(self):
method get_gripper_joint_names (line 129) | def get_gripper_joint_names(self):
method get_robot_name (line 148) | def get_robot_name(self):
method get_joint_limits (line 166) | def get_joint_limits(self):
FILE: franka_interface/src/motion_controller_interface.cpp
type franka_interface (line 32) | namespace franka_interface {
FILE: franka_interface/src/robot_state_controller.cpp
function convertArrayToTf (line 47) | tf::Transform convertArrayToTf(const std::array<double, 16>& transform) {
function errorsToMessage (line 54) | franka_msgs::Errors errorsToMessage(const franka::Errors& error) {
type franka_interface (line 162) | namespace franka_interface {
FILE: franka_interface/tests/demo_joint_positions_keyboard.py
function map_keyboard (line 58) | def map_keyboard():
function main (line 136) | def main():
FILE: franka_moveit/scripts/create_demo_planning_scene.py
function main (line 71) | def main():
FILE: franka_moveit/src/franka_moveit/extended_planning_scene_interface.py
class ExtendedPlanningSceneInterface (line 29) | class ExtendedPlanningSceneInterface(moveit_commander.PlanningSceneInter...
method __init__ (line 33) | def __init__(self):
method add_box (line 38) | def add_box(self, name, pose, size, timeout = 5):
method _wait_for_state_update (line 58) | def _wait_for_state_update(self, object_name, object_is_known=False, o...
method remove_box (line 74) | def remove_box(self, box_name, timeout=5):
FILE: franka_moveit/src/franka_moveit/movegroup_interface.py
function all_close (line 37) | def all_close(goal, actual, tolerance):
class PandaMoveGroupInterface (line 59) | class PandaMoveGroupInterface:
method __init__ (line 61) | def __init__(self, use_panda_hand_link=False):
method robot_state_interface (line 111) | def robot_state_interface(self):
method scene (line 122) | def scene(self):
method arm_group (line 134) | def arm_group(self):
method gripper_group (line 146) | def gripper_group(self):
method go_to_joint_positions (line 157) | def go_to_joint_positions(self, positions, wait = True, tolerance = 0....
method go_to_cartesian_pose (line 190) | def go_to_cartesian_pose(self, pose, ee_link="", wait=True):
method plan_cartesian_path (line 207) | def plan_cartesian_path(self, poses):
method set_velocity_scale (line 228) | def set_velocity_scale(self, value, group = "arm"):
method plan_joint_path (line 243) | def plan_joint_path(self, joint_position):
method close_gripper (line 253) | def close_gripper(self, wait = False):
method open_gripper (line 269) | def open_gripper(self, wait = False):
method display_trajectory (line 284) | def display_trajectory(self, plan):
method move_to_neutral (line 298) | def move_to_neutral(self, wait = True):
method execute_plan (line 308) | def execute_plan(self, plan, group = "arm", wait = True):
FILE: franka_moveit/src/franka_moveit/utils.py
function create_pose_msg (line 5) | def create_pose_msg(position, orientation):
function create_pose_stamped_msg (line 35) | def create_pose_stamped_msg(position, orientation, frame = "world"):
FILE: franka_moveit/tests/test_scene_interface.py
function main (line 35) | def main():
FILE: franka_ros_controllers/include/franka_ros_controllers/effort_joint_impedance_controller.h
function coriolis_factor_ (line 79) | double coriolis_factor_{1.0};
FILE: franka_ros_controllers/include/franka_ros_controllers/effort_joint_position_controller.h
function coriolis_factor_ (line 74) | double coriolis_factor_{1.0};
FILE: franka_ros_controllers/include/franka_ros_controllers/effort_joint_torque_controller.h
function coriolis_factor_ (line 69) | double coriolis_factor_{1.0};
FILE: franka_ros_controllers/include/franka_ros_controllers/position_joint_position_controller.h
function filter_factor_ (line 73) | double filter_factor_{0.01};
FILE: franka_ros_controllers/include/franka_ros_controllers/velocity_joint_velocity_controller.h
function filter_factor_ (line 73) | double filter_factor_{0.01};
FILE: franka_ros_controllers/src/effort_joint_impedance_controller.cpp
type franka_ros_controllers (line 37) | namespace franka_ros_controllers {
FILE: franka_ros_controllers/src/effort_joint_position_controller.cpp
type franka_ros_controllers (line 37) | namespace franka_ros_controllers {
FILE: franka_ros_controllers/src/effort_joint_torque_controller.cpp
type franka_ros_controllers (line 35) | namespace franka_ros_controllers {
FILE: franka_ros_controllers/src/position_joint_position_controller.cpp
type franka_ros_controllers (line 37) | namespace franka_ros_controllers {
FILE: franka_ros_controllers/src/velocity_joint_velocity_controller.cpp
type franka_ros_controllers (line 37) | namespace franka_ros_controllers {
FILE: franka_tools/src/franka_tools/collision_behaviour_interface.py
class CollisionBehaviourInterface (line 40) | class CollisionBehaviourInterface(object):
method __init__ (line 47) | def __init__(self):
method set_ft_contact_collision_behaviour (line 56) | def set_ft_contact_collision_behaviour(self, torque_lower = None, torq...
method set_force_threshold_for_contact (line 91) | def set_force_threshold_for_contact(self, cartesian_force_values):
method set_force_threshold_for_collision (line 100) | def set_force_threshold_for_collision(self, cartesian_force_values):
method set_collision_threshold (line 109) | def set_collision_threshold(self, joint_torques = None, cartesian_forc...
method set_contact_threshold (line 120) | def set_contact_threshold(self, joint_torques = None, cartesian_forces...
FILE: franka_tools/src/franka_tools/controller_manager_interface.py
function _resolve_controllers_ns (line 40) | def _resolve_controllers_ns(cm_ns):
function _append_ns (line 65) | def _append_ns(in_ns, suffix):
function _rosparam_controller_type (line 80) | def _rosparam_controller_type(ctrls_ns, ctrl_name):
function _get_controller_name_from_rosparam_server (line 93) | def _get_controller_name_from_rosparam_server(rosparam_name):
class ControllerStateInfo (line 108) | class ControllerStateInfo:
method __init__ (line 112) | def __init__(self, controller_state_msg):
class FrankaControllerManagerInterface (line 125) | class FrankaControllerManagerInterface(object):
method __init__ (line 149) | def __init__(self, ns="franka_ros_interface", synchronous_pub = False,...
method _clean_shutdown (line 214) | def _clean_shutdown(self):
method _on_controller_state (line 220) | def _on_controller_state(self, msg):
method _assert_one_active_controller (line 230) | def _assert_one_active_controller(self):
method load_controller (line 236) | def load_controller(self, name):
method unload_controller (line 245) | def unload_controller(self, name):
method start_controller (line 254) | def start_controller(self, name):
method get_controller_state (line 272) | def get_controller_state(self):
method stop_controller (line 280) | def stop_controller(self, name):
method list_loaded_controllers (line 295) | def list_loaded_controllers(self):
method list_controller_types (line 307) | def list_controller_types(self):
method list_controllers (line 319) | def list_controllers(self):
method controller_dict (line 349) | def controller_dict(self):
method set_motion_controller (line 364) | def set_motion_controller(self, controller_name):
method is_running (line 400) | def is_running(self, controller_name):
method is_loaded (line 415) | def is_loaded(self, controller_name):
method list_motion_controllers (line 430) | def list_motion_controllers(self):
method list_active_controllers (line 446) | def list_active_controllers(self, only_motion_controllers = False):
method list_active_controller_names (line 470) | def list_active_controller_names(self, only_motion_controllers = False):
method get_controller_config_client (line 482) | def get_controller_config_client(self, controller_name):
method get_current_controller_config_client (line 498) | def get_current_controller_config_client(self):
method list_controller_names (line 515) | def list_controller_names(self):
method joint_velocity_controller (line 533) | def joint_velocity_controller(self):
method joint_position_controller (line 544) | def joint_position_controller(self):
method joint_torque_controller (line 555) | def joint_torque_controller(self):
method joint_impedance_controller (line 566) | def joint_impedance_controller(self):
method effort_joint_position_controller (line 577) | def effort_joint_position_controller(self):
method joint_trajectory_controller (line 588) | def joint_trajectory_controller(self):
method current_controller (line 603) | def current_controller(self):
method default_controller (line 614) | def default_controller(self):
FILE: franka_tools/src/franka_tools/controller_param_config_client.py
class ControllerParamConfigClient (line 36) | class ControllerParamConfigClient:
method __init__ (line 44) | def __init__(self, controller_name):
method is_running (line 53) | def is_running(self):
method start (line 62) | def start(self, timeout = 5):
method _log_update (line 77) | def _log_update(self, config):
method update_config (line 88) | def update_config(self, **kwargs):
method set_controller_gains (line 99) | def set_controller_gains(self, k_gains, d_gains = None):
method set_joint_motion_smoothing_parameter (line 130) | def set_joint_motion_smoothing_parameter(self, value):
method get_joint_motion_smoothing_parameter (line 142) | def get_joint_motion_smoothing_parameter(self, timeout = 5):
method get_config (line 155) | def get_config(self, timeout = 5):
method get_controller_gains (line 166) | def get_controller_gains(self, timeout = 5):
method get_parameter_descriptions (line 199) | def get_parameter_descriptions(self, timeout = 5):
FILE: franka_tools/src/franka_tools/frames_interface.py
class FrankaFramesInterface (line 43) | class FrankaFramesInterface(object):
method __init__ (line 55) | def __init__(self):
method set_EE_frame (line 60) | def set_EE_frame(self, frame):
method _update_frame_data (line 74) | def _update_frame_data(self, EE_frame_transformation, K_frame_transfor...
method _assert_frame_validity (line 86) | def _assert_frame_validity(self, frame):
method get_link_tf (line 100) | def get_link_tf(self, frame_name, timeout=5.0, parent='panda_NE'):
method frames_are_same (line 141) | def frames_are_same(self, frame1, frame2):
method EE_frame_already_set (line 155) | def EE_frame_already_set(self, frame):
method set_EE_at_frame (line 164) | def set_EE_at_frame(self, frame_name, timeout=5.0):
method get_EE_frame (line 180) | def get_EE_frame(self, as_mat=False):
method reset_EE_frame (line 191) | def reset_EE_frame(self):
method EE_frame_is_reset (line 201) | def EE_frame_is_reset(self):
method _request_setEE_service (line 205) | def _request_setEE_service(self, trans_mat):
method set_K_frame (line 220) | def set_K_frame(self, frame):
method set_K_at_frame (line 234) | def set_K_at_frame(self, frame_name, timeout=5.0):
method get_K_frame (line 249) | def get_K_frame(self, as_mat=False):
method reset_K_frame (line 260) | def reset_K_frame(self):
method K_frame_is_reset (line 271) | def K_frame_is_reset(self):
method _request_setK_service (line 275) | def _request_setK_service(self, trans_mat):
FILE: franka_tools/src/franka_tools/joint_trajectory_action_client.py
class JointTrajectoryActionClient (line 40) | class JointTrajectoryActionClient(object):
method __init__ (line 46) | def __init__(self, joint_names, controller_name = "position_joint_traj...
method add_point (line 64) | def add_point(self, positions, time, velocities = None):
method start (line 89) | def start(self):
method stop (line 96) | def stop(self):
method wait (line 102) | def wait(self, timeout=15.0):
method result (line 111) | def result(self):
method clear (line 117) | def clear(self):
Condensed preview — 85 files, each showing path, character count, and a content snippet. Download the .json file or copy for the full structured content (419K chars).
[
{
"path": ".github/workflows/auto-comment.yml",
"chars": 925,
"preview": "name: Auto Comment\non: [issues, pull_request]\njobs:\n run:\n runs-on: ubuntu-latest\n steps:\n - uses: wow-actio"
},
{
"path": ".gitignore",
"chars": 552,
"preview": "*.py[cod]\n__pycache__\n*$py.class\n\n# C extensions\n*.so\n\n# Packages\n*.egg\n*.egg-info\ndist\nbuild\neggs\nparts\nbin\nvar\nsdist\nd"
},
{
"path": ".travis.yml",
"chars": 5766,
"preview": "# Generic .travis.yml file for running continuous integration on Travis-CI for\n# any ROS package.\n#\n# Available here:\n# "
},
{
"path": "CMakeLists.txt",
"chars": 2161,
"preview": "# toplevel CMakeLists.txt for a catkin workspace\n# catkin/cmake/toplevel.cmake\n\ncmake_minimum_required(VERSION 2.8.3)\n\ns"
},
{
"path": "LICENSE",
"chars": 11346,
"preview": "\n Apache License\n Version 2.0, January 2004\n "
},
{
"path": "NOTICE",
"chars": 638,
"preview": "\n franka_ros_interface package\n\n Copyright 2019-2021 Saif Sidhik\n\n Licensed under the Apache Lice"
},
{
"path": "README.md",
"chars": 14192,
"preview": "# Franka ROS Interface [](https://github.com/justagist/f"
},
{
"path": "cmake/ClangTools.cmake",
"chars": 2147,
"preview": "include(CMakeParseArguments)\n\nfind_program(CLANG_FORMAT_PROG clang-format-5.0 DOC \"'clang-format' executable\")\nif(CLANG_"
},
{
"path": "franka.sh",
"chars": 10937,
"preview": "#!/bin/bash\n\n# Copyright (c) 2019-2021, Saif Sidhik\n# Copyright (c) 2013-2016, Rethink Robotics\n# All rights reserved.\n\n"
},
{
"path": "franka_common/franka_core_msgs/CMakeLists.txt",
"chars": 928,
"preview": "cmake_minimum_required(VERSION 2.8.3)\n\nproject(franka_core_msgs)\n\nfind_package(catkin REQUIRED COMPONENTS\n messag"
},
{
"path": "franka_common/franka_core_msgs/msg/EndPointState.msg",
"chars": 765,
"preview": "std_msgs/Header header\n\nfloat64[16] O_T_EE # Measured end effector pose in base frame\n\n# ----- Moved ee velocity to robo"
},
{
"path": "franka_common/franka_core_msgs/msg/JointCommand.msg",
"chars": 698,
"preview": "Header header\n\nint32 mode # Mode in which to command arm\n\nstring[] names # Joint names order for com"
},
{
"path": "franka_common/franka_core_msgs/msg/JointControllerStates.msg",
"chars": 158,
"preview": "Header header\n\nstring controller_name\n\nstring[] names # Joint names order for command\n\ncontrol_msgs/JointControl"
},
{
"path": "franka_common/franka_core_msgs/msg/JointLimits.msg",
"chars": 410,
"preview": "# names of the joints\nstring[] joint_names\n\n# lower bound on the angular position in radians\nfloat64[] position_lower\n\n#"
},
{
"path": "franka_common/franka_core_msgs/msg/RobotState.msg",
"chars": 2332,
"preview": "std_msgs/Header header\n\nfloat64[6] cartesian_collision\nfloat64[6] cartesian_contact\nfloat64[6] O_dP_EE # EE vel computed"
},
{
"path": "franka_common/franka_core_msgs/package.xml",
"chars": 1084,
"preview": "<?xml version=\"1.0\"?>\n<package>\n <name>franka_core_msgs</name>\n <version>0.7.1</version>\n <description>\n Messages "
},
{
"path": "franka_interface/CMakeLists.txt",
"chars": 3771,
"preview": "cmake_minimum_required(VERSION 2.8.3)\nproject(franka_interface)\n\nset(CMAKE_BUILD_TYPE Release)\n\nset(CMAKE_CXX_STANDARD 1"
},
{
"path": "franka_interface/README.md",
"chars": 969,
"preview": "# franka_interface\n\n**See induvidual source files for more detailed documentation.**\n\n### ArmInterface\n- Interface class"
},
{
"path": "franka_interface/config/basic_controllers.yaml",
"chars": 928,
"preview": "# load the internal joint trajectory controller, and the custom robot state controller\n\nposition_joint_trajectory_contro"
},
{
"path": "franka_interface/config/robot_config.yaml",
"chars": 4661,
"preview": "# global robot configuration and details\n\nrobot_config:\n\n joint_names:\n - panda_joint1\n - panda_joint2\n "
},
{
"path": "franka_interface/include/franka_interface/motion_controller_interface.h",
"chars": 4124,
"preview": "/***************************************************************************\n* Adapted from sawyer_simulator package\n\n*\n"
},
{
"path": "franka_interface/include/franka_interface/robot_state_controller.h",
"chars": 3983,
"preview": "/***************************************************************************\n* Adapted from robot_state_controller.cpp ("
},
{
"path": "franka_interface/launch/interface.launch",
"chars": 5139,
"preview": "<?xml version=\"1.0\" ?>\n<launch>\n <rosparam command=\"load\" file=\"$(find franka_ros_controllers)/config/ros_controllers.y"
},
{
"path": "franka_interface/package.xml",
"chars": 1295,
"preview": "<?xml version=\"1.0\"?>\n<package format=\"2\">>\n <name>franka_interface</name>\n <version>0.7.1</version>\n <description>\n "
},
{
"path": "franka_interface/scripts/enable_robot.py",
"chars": 3079,
"preview": "#!/usr/bin/env python\n\n# /***************************************************************************\n\n# \n# @package: fr"
},
{
"path": "franka_interface/scripts/move_to_neutral.py",
"chars": 1274,
"preview": "#!/usr/bin/env python\n\n# /***************************************************************************\n\n# \n# @package: fr"
},
{
"path": "franka_interface/scripts/simple_gripper.py",
"chars": 3058,
"preview": "#!/usr/bin/env python\n\n# /***************************************************************************\n\n# \n# @package: fr"
},
{
"path": "franka_interface/setup.py",
"chars": 246,
"preview": "#!/usr/bin/env python\nfrom distutils.core import setup\nfrom catkin_pkg.python_setup import generate_distutils_setup\n\nd ="
},
{
"path": "franka_interface/src/franka_control_node.cpp",
"chars": 6477,
"preview": "// /***************************************************************************\n// * Adapted from franka_control_node.cp"
},
{
"path": "franka_interface/src/franka_dataflow/__init__.py",
"chars": 819,
"preview": "# /***************************************************************************\n# Copyright (c) 2019-2021, Saif Sidhik\n# "
},
{
"path": "franka_interface/src/franka_dataflow/getch.py",
"chars": 1435,
"preview": "# Copyright (c) 2013-2018, Rethink Robotics Inc.\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# y"
},
{
"path": "franka_interface/src/franka_dataflow/wait_for.py",
"chars": 2286,
"preview": "# /***************************************************************************\n# Modified from: Rethink Robotics Intera "
},
{
"path": "franka_interface/src/franka_interface/__init__.py",
"chars": 885,
"preview": "# /***************************************************************************\n# Copyright (c) 2019-2021, Saif Sidhik\n \n"
},
{
"path": "franka_interface/src/franka_interface/arm.py",
"chars": 38986,
"preview": "# /***************************************************************************\n\n#\n# @package: franka_interface\n# @metapa"
},
{
"path": "franka_interface/src/franka_interface/gripper.py",
"chars": 14007,
"preview": "# /***************************************************************************\n\n# \n# @package: franka_interface\n# @metap"
},
{
"path": "franka_interface/src/franka_interface/robot_enable.py",
"chars": 4015,
"preview": "# /***************************************************************************\n\n# \n# @package: franka_interface\n# @metap"
},
{
"path": "franka_interface/src/franka_interface/robot_params.py",
"chars": 8328,
"preview": "# /***************************************************************************\n\n# \n# @package: franka_interface\n# @metap"
},
{
"path": "franka_interface/src/motion_controller_interface.cpp",
"chars": 10261,
"preview": "/***************************************************************************\n* Adapted from arm_controller_interface.cpp"
},
{
"path": "franka_interface/src/robot_state_controller.cpp",
"chars": 27849,
"preview": "/***************************************************************************\n* Adapted from robot_state_publisher.cpp (f"
},
{
"path": "franka_interface/state_controller_plugin.xml",
"chars": 330,
"preview": "<library path=\"lib/libcustom_franka_state_controller\">\n <class name=\"franka_interface/CustomFrankaStateController\" type"
},
{
"path": "franka_interface/tests/demo_interface.py",
"chars": 1868,
"preview": "import rospy\nimport numpy as np\nfrom franka_interface import ArmInterface\n\n\"\"\"\n:info:\n Demo showing ways to use API. "
},
{
"path": "franka_interface/tests/demo_joint_positions_keyboard.py",
"chars": 5732,
"preview": "#!/usr/bin/env python\n\n# /***************************************************************************\n\n# \n# @package: fr"
},
{
"path": "franka_interface/tests/test_zeroG.py",
"chars": 1335,
"preview": "import rospy\nimport numpy as np\nfrom franka_interface import ArmInterface\n\n\"\"\"\n:info:\n This code is for testing the t"
},
{
"path": "franka_moveit/CMakeLists.txt",
"chars": 518,
"preview": "cmake_minimum_required(VERSION 2.8.3)\nproject(franka_moveit)\n\nfind_package(catkin REQUIRED COMPONENTS\n panda_moveit_c"
},
{
"path": "franka_moveit/README.md",
"chars": 523,
"preview": "# franka_moveit\n\nSome helper classes which interfaces moveit with the Franka Emika Panda robot.\n\n### PandaMoveGroupInter"
},
{
"path": "franka_moveit/config/moveit_demo.rviz",
"chars": 11392,
"preview": "Panels:\n - Class: rviz/Displays\n Help Height: 84\n Name: Displays\n Property Tree Widget:\n Expanded:\n "
},
{
"path": "franka_moveit/launch/demo_moveit.launch",
"chars": 2639,
"preview": "<launch>\n\n <!-- By default, we do not start a database (it can be large) -->\n <arg name=\"db\" default=\"false\" />\n <!--"
},
{
"path": "franka_moveit/package.xml",
"chars": 500,
"preview": "<?xml version=\"1.0\"?>\n<package>\n <name>franka_moveit</name>\n <version>0.7.1</version>\n <description>\n franka_movei"
},
{
"path": "franka_moveit/scripts/create_demo_planning_scene.py",
"chars": 3226,
"preview": "#!/usr/bin/env python\n\n# /***************************************************************************\n\n# \n# @package: fr"
},
{
"path": "franka_moveit/setup.py",
"chars": 225,
"preview": "#!/usr/bin/env python\nfrom distutils.core import setup\nfrom catkin_pkg.python_setup import generate_distutils_setup\n\nd ="
},
{
"path": "franka_moveit/src/franka_moveit/__init__.py",
"chars": 895,
"preview": "# /***************************************************************************\n# Copyright (c) 2019-2021, Saif Sidhik\n \n"
},
{
"path": "franka_moveit/src/franka_moveit/extended_planning_scene_interface.py",
"chars": 3345,
"preview": "# /***************************************************************************\n\n# \n# @package: franka_moveit\n# @metapack"
},
{
"path": "franka_moveit/src/franka_moveit/movegroup_interface.py",
"chars": 13393,
"preview": "# /***************************************************************************\n\n# \n# @package: franka_moveit\n# @metapack"
},
{
"path": "franka_moveit/src/franka_moveit/utils.py",
"chars": 1917,
"preview": "\nimport geometry_msgs.msg\nimport quaternion\n\ndef create_pose_msg(position, orientation):\n \"\"\"\n Create Pose mes"
},
{
"path": "franka_moveit/tests/test_movegroup_interface.py",
"chars": 219,
"preview": "import rospy\nfrom franka_moveit import PandaMoveGroupInterface\n\nif __name__ == '__main__':\n \n\n rospy.init_node(\"te"
},
{
"path": "franka_moveit/tests/test_scene_interface.py",
"chars": 1761,
"preview": "#!/usr/bin/env python\n\nimport sys\nimport rospy\nimport moveit_commander\nfrom builtins import input\n\nfrom franka_moveit im"
},
{
"path": "franka_ros_controllers/CMakeLists.txt",
"chars": 2514,
"preview": "cmake_minimum_required(VERSION 2.8.3)\nproject(franka_ros_controllers)\n\nset(CMAKE_BUILD_TYPE Release)\n\nset(CMAKE_CXX_STAN"
},
{
"path": "franka_ros_controllers/README.md",
"chars": 751,
"preview": "# franka_ros_controllers\n\nControllers for the Franka Emika Panda robot using ROS topics.\n\n### Features:\n\n- Includes join"
},
{
"path": "franka_ros_controllers/cfg/joint_controller_params.cfg",
"chars": 2298,
"preview": "#!/usr/bin/env python\nPACKAGE = \"franka_ros_controllers\"\n\nfrom dynamic_reconfigure.parameter_generator_catkin import *\n\n"
},
{
"path": "franka_ros_controllers/config/ros_controllers.yaml",
"chars": 2110,
"preview": "position_joint_position_controller:\n type: franka_ros_controllers/PositionJointPositionController\n joint_names:\n "
},
{
"path": "franka_ros_controllers/controller_plugins.xml",
"chars": 1789,
"preview": "<library path=\"lib/libfranka_ros_controllers\">\n <class name=\"franka_ros_controllers/EffortJointImpedanceController\"\n "
},
{
"path": "franka_ros_controllers/include/franka_ros_controllers/effort_joint_impedance_controller.h",
"chars": 4158,
"preview": "/***************************************************************************\n\n*\n* @package: franka_ros_controllers\n* @me"
},
{
"path": "franka_ros_controllers/include/franka_ros_controllers/effort_joint_position_controller.h",
"chars": 3894,
"preview": "/***************************************************************************\n\n*\n* @package: franka_ros_controllers\n* @me"
},
{
"path": "franka_ros_controllers/include/franka_ros_controllers/effort_joint_torque_controller.h",
"chars": 3137,
"preview": "/***************************************************************************\n\n*\n* @package: franka_ros_controllers\n* @me"
},
{
"path": "franka_ros_controllers/include/franka_ros_controllers/position_joint_position_controller.h",
"chars": 3377,
"preview": "/***************************************************************************\n\n*\n* @package: franka_ros_controllers\n* @me"
},
{
"path": "franka_ros_controllers/include/franka_ros_controllers/velocity_joint_velocity_controller.h",
"chars": 3523,
"preview": "/***************************************************************************\n\n*\n* @package: franka_ros_controllers\n* @me"
},
{
"path": "franka_ros_controllers/package.xml",
"chars": 1089,
"preview": "<?xml version=\"1.0\"?>\n<package format=\"2\">\n <name>franka_ros_controllers</name>\n <version>0.7.1</version>\n <descripti"
},
{
"path": "franka_ros_controllers/scripts/test_controller.py",
"chars": 1419,
"preview": "import rospy\nfrom franka_interface import ArmInterface\nimport numpy as np\nfrom builtins import input\n# import matplotlib"
},
{
"path": "franka_ros_controllers/src/effort_joint_impedance_controller.cpp",
"chars": 15276,
"preview": "/***************************************************************************\n\n*\n* @package: franka_ros_controllers\n* @me"
},
{
"path": "franka_ros_controllers/src/effort_joint_position_controller.cpp",
"chars": 12674,
"preview": "/***************************************************************************\n\n*\n* @package: franka_ros_controllers\n* @me"
},
{
"path": "franka_ros_controllers/src/effort_joint_torque_controller.cpp",
"chars": 9025,
"preview": "/***************************************************************************\n\n*\n* @package: franka_ros_controllers\n* @me"
},
{
"path": "franka_ros_controllers/src/position_joint_position_controller.cpp",
"chars": 9123,
"preview": "/***************************************************************************\n\n*\n* @package: franka_ros_controllers\n* @me"
},
{
"path": "franka_ros_controllers/src/velocity_joint_velocity_controller.cpp",
"chars": 8552,
"preview": "/***************************************************************************\n\n*\n* @package: franka_ros_controllers\n* @me"
},
{
"path": "franka_ros_interface/CMakeLists.txt",
"chars": 119,
"preview": "cmake_minimum_required(VERSION 2.8.3)\nproject(franka_ros_interface)\nfind_package(catkin REQUIRED)\ncatkin_metapackage()\n"
},
{
"path": "franka_ros_interface/package.xml",
"chars": 748,
"preview": "<?xml version=\"1.0\"?>\n<package>\n <name>franka_ros_interface</name>\n <version>0.7.1</version>\n <description>\n frank"
},
{
"path": "franka_tools/CMakeLists.txt",
"chars": 554,
"preview": "cmake_minimum_required(VERSION 2.8.3)\nproject(franka_tools)\n\n## Find catkin macros and libraries\n## if COMPONENTS list l"
},
{
"path": "franka_tools/README.md",
"chars": 1044,
"preview": "# franka_tools\n\nSome helper classes for controlling and handling the Franka Emika Panda robot.\n\n## FrankaControllerManag"
},
{
"path": "franka_tools/package.xml",
"chars": 1189,
"preview": "<?xml version=\"1.0\"?>\n<package>\n <name>franka_tools</name>\n <version>0.7.1</version>\n <description>The franka_tools p"
},
{
"path": "franka_tools/setup.py",
"chars": 224,
"preview": "#!/usr/bin/env python\nfrom distutils.core import setup\nfrom catkin_pkg.python_setup import generate_distutils_setup\n\nd ="
},
{
"path": "franka_tools/src/franka_tools/__init__.py",
"chars": 1082,
"preview": "# /***************************************************************************\n# Copyright (c) 2019-2021, Saif Sidhik\n \n"
},
{
"path": "franka_tools/src/franka_tools/collision_behaviour_interface.py",
"chars": 6869,
"preview": "# /***************************************************************************\n\n# \n# @package: franka_tools\n# @metapacka"
},
{
"path": "franka_tools/src/franka_tools/controller_manager_interface.py",
"chars": 25327,
"preview": "# /***************************************************************************\n\n# \n# @package: franka_tools\n# @metapacka"
},
{
"path": "franka_tools/src/franka_tools/controller_param_config_client.py",
"chars": 7542,
"preview": "# /***************************************************************************\n\n# \n# @package: franka_tools\n# @metapacka"
},
{
"path": "franka_tools/src/franka_tools/frames_interface.py",
"chars": 12812,
"preview": "# /***************************************************************************\n\n#\n# @package: franka_tools\n# @metapackag"
},
{
"path": "franka_tools/src/franka_tools/joint_trajectory_action_client.py",
"chars": 4367,
"preview": "# /***************************************************************************\n\n# \n# @package: franka_tools\n# @metapacka"
}
]
About this extraction
This page contains the full source code of the justagist/franka_ros_interface GitHub repository, extracted and formatted as plain text for AI agents and large language models (LLMs). The extraction includes 85 files (390.0 KB), approximately 94.9k tokens, and a symbol index with 233 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.