[ { "path": ".github/workflows/auto-comment.yml", "content": "name: Auto Comment\non: [issues, pull_request]\njobs:\n run:\n runs-on: ubuntu-latest\n steps:\n - uses: wow-actions/auto-comment@v1\n with:\n GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }}\n issuesOpened: |\n Hi @{{ author }}, thank you for using this repository. Unfortunately, I am unable to maintain this package any more, sorry!\n If you are interested in contributing to this repository or are willing to maintain it, please contact me at mail@saifsidhik.page.\n -- Saif\n\n pullRequestOpened: |\n Hi @{{ author }},\n Thank you for the pull request. Unfortunately, I am unable to maintain this package any more, sorry!\n 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.\n -- Saif\n" }, { "path": ".gitignore", "content": "*.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\ndevelop-eggs\n.installed.cfg\nlib\nlib64\n\n# Installer logs\npip-log.txt\n\n# Unit test / coverage reports\n.coverage\n.tox\nnosetests.xml\n\n# Translations\n*.mo\n\n# Mr Developer\n.mr.developer.cfg\n.project\n.cproject\ncmake_install.cmake\n.pydevproject\n*.swp\n*.swo\nCATKIN_IGNORE\ncatkin\ncatkin_generated\ndevel\n\ncpp\ndocs_src\ndoc_src\ndocs_build\nmsg_gen\nsrv_gen\n*.smoketest\n*.cfgc\n*_msgs/src/\n*/src/**/cfg\n*/*/src/**/*\n\nfranka_interface/scripts/test.py\n" }, { "path": ".travis.yml", "content": "# Generic .travis.yml file for running continuous integration on Travis-CI for\n# any ROS package.\n#\n# Available here:\n# - https://github.com/felixduvallet/ros-travis-integration\n#\n# This installs ROS on a clean Travis-CI virtual machine, creates a ROS\n# workspace, resolves all listed dependencies, and sets environment variables\n# (setup.bash). Then, it compiles the entire ROS workspace (ensuring there are\n# no compilation errors), and runs all the tests. If any of the compilation/test\n# phases fail, the build is marked as a failure.\n#\n# We handle two types of package dependencies specified in the package manifest:\n# - system dependencies that can be installed using `rosdep`, including other\n# ROS packages and system libraries. These dependencies must be known to\n# `rosdistro` and are installed using apt-get.\n# - package dependencies that must be checked out from source. These are handled by\n# `wstool`, and should be listed in a file named dependencies.rosinstall.\n#\n\n# There are envioronment variables you may want to change, such as ROS_DISTRO,\n# ROSINSTALL_FILE, and the CATKIN_OPTIONS file. See the README.md for more\n# information on these flags, and\n# https://docs.travis-ci.com/user/environment-variables/ for information about\n# Travis environment variables in general.\n#\n# Author: Felix Duvallet \n\n# NOTE: The build lifecycle on Travis.ci is something like this:\n# before_install\n# install\n# before_script\n# script\n# after_success or after_failure\n# after_script\n# OPTIONAL before_deploy\n# OPTIONAL deploy\n# OPTIONAL after_deploy\n\n################################################################################\n\nsudo: required\ncache:\n - apt\n\n# Build all valid Ubuntu/ROS combinations available on Travis VMs.\nlanguage: generic\nmatrix:\n include:\n - name: \"focal noetic\"\n dist: focal\n env: ROS_DISTRO=noetic\n - name: \"Bionic melodic\"\n dist: bionic\n env: ROS_DISTRO=melodic\n\n# Configuration variables. All variables are global now, but this can be used to\n# trigger a build matrix for different ROS distributions if desired.\nenv:\n global:\n - ROS_CI_DESKTOP=\"`lsb_release -cs`\" # e.g. [trusty|xenial|...]\n - CI_SOURCE_PATH=$(pwd)\n - ROSINSTALL_FILE=$CI_SOURCE_PATH/dependencies.rosinstall\n - ROS_PARALLEL_JOBS='-j8 -l6'\n # Set the python path manually to include /usr/-/python2.7/dist-packages\n # as this is where apt-get installs python packages.\n\n################################################################################\n\n# Install system dependencies, namely a very barebones ROS setup.\nbefore_install:\n - if [[ $ROS_DISTRO == \"noetic\" ]]; then export PYMODULE=python3; else export PYMODULE=python; fi;\n - export PYTHONPATH=$PYTHONPATH:/usr/lib/${PYMODULE}/dist-packages:/usr/local/lib/${PYMODULE}/dist-packages\n - sudo sh -c \"echo \\\"deb http://packages.ros.org/ros/ubuntu $ROS_CI_DESKTOP main\\\" > /etc/apt/sources.list.d/ros-latest.list\"\n - sudo apt-key adv --keyserver 'hkp://keyserver.ubuntu.com:80' --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654\n - sudo apt-get update -qq\n - sudo apt-get install dpkg\n - sudo apt-get install -y ${PYMODULE}-catkin-pkg ${PYMODULE}-rosdep ${PYMODULE}-wstool ros-$ROS_DISTRO-desktop-full ros-$ROS_DISTRO-combined-robot-hw\n - source /opt/ros/$ROS_DISTRO/setup.bash\n - sudo apt install -y ros-$ROS_DISTRO-libfranka\n - if [[ $ROS_DISTRO == \"melodic\" ]]; then sudo apt install -y ros-$ROS_DISTRO-franka-ros; fi;\n - sudo apt install -y ${PYMODULE}-catkin-tools ${PYMODULE}-osrf-pycommon\n # Prepare rosdep to install dependencies.\n - sudo rosdep init\n - rosdep update --include-eol-distros # Support EOL distros.\n\n# Create a catkin workspace with the package under integration.\ninstall:\n - mkdir -p ~/catkin_ws/src\n - cd ~/catkin_ws/src\n - catkin_init_workspace\n # Create the devel/setup.bash (run catkin_make with an empty workspace) and\n # source it to set the path variables.\n - cd ~/catkin_ws\n - catkin build\n - source devel/setup.bash\n # Add the package under integration to the workspace using a symlink.\n - cd ~/catkin_ws/src\n - ln -s $CI_SOURCE_PATH .\n\n# Install all dependencies, using wstool first and rosdep second.\n# wstool looks for a ROSINSTALL_FILE defined in the environment variables.\nbefore_script:\n # source dependencies: install using wstool.\n - cd ~/catkin_ws/src\n - wstool init\n - if [[ -f $ROSINSTALL_FILE ]] ; then wstool merge $ROSINSTALL_FILE ; fi\n - wstool up\n - 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;\n # package depdencies: install using rosdep.\n - cd ~/catkin_ws\n # - rosdep install -y --from-paths src --ignore-src --rosdistro $ROS_DISTRO\n\n# Compile and test (mark the build as failed if any step fails). If the\n# CATKIN_OPTIONS file exists, use it as an argument to catkin_make, for example\n# to blacklist certain packages.\n#\n# NOTE on testing: `catkin_make run_tests` will show the output of the tests\n# (gtest, nosetest, etc..) but always returns 0 (success) even if a test\n# fails. Running `catkin_test_results` aggregates all the results and returns\n# non-zero when a test fails (which notifies Travis the build failed).\nscript:\n - source /opt/ros/$ROS_DISTRO/setup.bash\n - cd ~/catkin_ws\n # seems to need separate builds for succeeding when using franka_ros from source\n - if [[ $ROS_DISTRO == \"noetic\" ]]; then catkin build franka_msgs; source devel/setup.bash; catkin build franka_ros; fi;\n \n - source devel/setup.bash\n # Run the tests, ensuring the path is set correctly.\n - catkin build franka_ros_interface #--catkin-make-args run_tests && catkin_test_results" }, { "path": "CMakeLists.txt", "content": "# toplevel CMakeLists.txt for a catkin workspace\n# catkin/cmake/toplevel.cmake\n\ncmake_minimum_required(VERSION 2.8.3)\n\nset(CATKIN_TOPLEVEL TRUE)\n\n# search for catkin within the workspace\nset(_cmd \"catkin_find_pkg\" \"catkin\" \"${CMAKE_SOURCE_DIR}\")\nexecute_process(COMMAND ${_cmd}\n RESULT_VARIABLE _res\n OUTPUT_VARIABLE _out\n ERROR_VARIABLE _err\n OUTPUT_STRIP_TRAILING_WHITESPACE\n ERROR_STRIP_TRAILING_WHITESPACE\n)\nif(NOT _res EQUAL 0 AND NOT _res EQUAL 2)\n # searching fot catkin resulted in an error\n string(REPLACE \";\" \" \" _cmd_str \"${_cmd}\")\n message(FATAL_ERROR \"Search for 'catkin' in workspace failed (${_cmd_str}): ${_err}\")\nendif()\n\n# include catkin from workspace or via find_package()\nif(_res EQUAL 0)\n set(catkin_EXTRAS_DIR \"${CMAKE_SOURCE_DIR}/${_out}/cmake\")\n # include all.cmake without add_subdirectory to let it operate in same scope\n include(${catkin_EXTRAS_DIR}/all.cmake NO_POLICY_SCOPE)\n add_subdirectory(\"${_out}\")\n\nelse()\n # use either CMAKE_PREFIX_PATH explicitly passed to CMake as a command line argument\n # or CMAKE_PREFIX_PATH from the environment\n if(NOT DEFINED CMAKE_PREFIX_PATH)\n if(NOT \"$ENV{CMAKE_PREFIX_PATH}\" STREQUAL \"\")\n if(NOT WIN32)\n string(REPLACE \":\" \";\" CMAKE_PREFIX_PATH $ENV{CMAKE_PREFIX_PATH})\n else()\n set(CMAKE_PREFIX_PATH $ENV{CMAKE_PREFIX_PATH})\n endif()\n endif()\n endif()\n\n # list of catkin workspaces\n set(catkin_search_path \"\")\n foreach(path ${CMAKE_PREFIX_PATH})\n if(EXISTS \"${path}/.catkin\")\n list(FIND catkin_search_path ${path} _index)\n if(_index EQUAL -1)\n list(APPEND catkin_search_path ${path})\n endif()\n endif()\n endforeach()\n\n # search for catkin in all workspaces\n set(CATKIN_TOPLEVEL_FIND_PACKAGE TRUE)\n find_package(catkin QUIET\n NO_POLICY_SCOPE\n PATHS ${catkin_search_path}\n NO_DEFAULT_PATH NO_CMAKE_FIND_ROOT_PATH)\n unset(CATKIN_TOPLEVEL_FIND_PACKAGE)\n\n if(NOT catkin_FOUND)\n 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.\")\n endif()\nendif()\n\ncatkin_workspace()\n" }, { "path": "LICENSE", "content": "\n Apache License\n Version 2.0, January 2004\n http://www.apache.org/licenses/\n\n TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION\n\n 1. Definitions.\n\n \"License\" shall mean the terms and conditions for use, reproduction,\n and distribution as defined by Sections 1 through 9 of this document.\n\n \"Licensor\" shall mean the copyright owner or entity authorized by\n the copyright owner that is granting the License.\n\n \"Legal Entity\" shall mean the union of the acting entity and all\n other entities that control, are controlled by, or are under common\n control with that entity. For the purposes of this definition,\n \"control\" means (i) the power, direct or indirect, to cause the\n direction or management of such entity, whether by contract or\n otherwise, or (ii) ownership of fifty percent (50%) or more of the\n outstanding shares, or (iii) beneficial ownership of such entity.\n\n \"You\" (or \"Your\") shall mean an individual or Legal Entity\n exercising permissions granted by this License.\n\n \"Source\" form shall mean the preferred form for making modifications,\n including but not limited to software source code, documentation\n source, and configuration files.\n\n \"Object\" form shall mean any form resulting from mechanical\n transformation or translation of a Source form, including but\n not limited to compiled object code, generated documentation,\n and conversions to other media types.\n\n \"Work\" shall mean the work of authorship, whether in Source or\n Object form, made available under the License, as indicated by a\n copyright notice that is included in or attached to the work\n (an example is provided in the Appendix below).\n\n \"Derivative Works\" shall mean any work, whether in Source or Object\n form, that is based on (or derived from) the Work and for which the\n editorial revisions, annotations, elaborations, or other modifications\n represent, as a whole, an original work of authorship. For the purposes\n of this License, Derivative Works shall not include works that remain\n separable from, or merely link (or bind by name) to the interfaces of,\n the Work and Derivative Works thereof.\n\n \"Contribution\" shall mean any work of authorship, including\n the original version of the Work and any modifications or additions\n to that Work or Derivative Works thereof, that is intentionally\n submitted to Licensor for inclusion in the Work by the copyright owner\n or by an individual or Legal Entity authorized to submit on behalf of\n the copyright owner. For the purposes of this definition, \"submitted\"\n means any form of electronic, verbal, or written communication sent\n to the Licensor or its representatives, including but not limited to\n communication on electronic mailing lists, source code control systems,\n and issue tracking systems that are managed by, or on behalf of, the\n Licensor for the purpose of discussing and improving the Work, but\n excluding communication that is conspicuously marked or otherwise\n designated in writing by the copyright owner as \"Not a Contribution.\"\n\n \"Contributor\" shall mean Licensor and any individual or Legal Entity\n on behalf of whom a Contribution has been received by Licensor and\n subsequently incorporated within the Work.\n\n 2. Grant of Copyright License. Subject to the terms and conditions of\n this License, each Contributor hereby grants to You a perpetual,\n worldwide, non-exclusive, no-charge, royalty-free, irrevocable\n copyright license to reproduce, prepare Derivative Works of,\n publicly display, publicly perform, sublicense, and distribute the\n Work and such Derivative Works in Source or Object form.\n\n 3. Grant of Patent License. Subject to the terms and conditions of\n this License, each Contributor hereby grants to You a perpetual,\n worldwide, non-exclusive, no-charge, royalty-free, irrevocable\n (except as stated in this section) patent license to make, have made,\n use, offer to sell, sell, import, and otherwise transfer the Work,\n where such license applies only to those patent claims licensable\n by such Contributor that are necessarily infringed by their\n Contribution(s) alone or by combination of their Contribution(s)\n with the Work to which such Contribution(s) was submitted. If You\n institute patent litigation against any entity (including a\n cross-claim or counterclaim in a lawsuit) alleging that the Work\n or a Contribution incorporated within the Work constitutes direct\n or contributory patent infringement, then any patent licenses\n granted to You under this License for that Work shall terminate\n as of the date such litigation is filed.\n\n 4. Redistribution. You may reproduce and distribute copies of the\n Work or Derivative Works thereof in any medium, with or without\n modifications, and in Source or Object form, provided that You\n meet the following conditions:\n\n (a) You must give any other recipients of the Work or\n Derivative Works a copy of this License; and\n\n (b) You must cause any modified files to carry prominent notices\n stating that You changed the files; and\n\n (c) You must retain, in the Source form of any Derivative Works\n that You distribute, all copyright, patent, trademark, and\n attribution notices from the Source form of the Work,\n excluding those notices that do not pertain to any part of\n the Derivative Works; and\n\n (d) If the Work includes a \"NOTICE\" text file as part of its\n distribution, then any Derivative Works that You distribute must\n include a readable copy of the attribution notices contained\n within such NOTICE file, excluding those notices that do not\n pertain to any part of the Derivative Works, in at least one\n of the following places: within a NOTICE text file distributed\n as part of the Derivative Works; within the Source form or\n documentation, if provided along with the Derivative Works; or,\n within a display generated by the Derivative Works, if and\n wherever such third-party notices normally appear. The contents\n of the NOTICE file are for informational purposes only and\n do not modify the License. You may add Your own attribution\n notices within Derivative Works that You distribute, alongside\n or as an addendum to the NOTICE text from the Work, provided\n that such additional attribution notices cannot be construed\n as modifying the License.\n\n You may add Your own copyright statement to Your modifications and\n may provide additional or different license terms and conditions\n for use, reproduction, or distribution of Your modifications, or\n for any such Derivative Works as a whole, provided Your use,\n reproduction, and distribution of the Work otherwise complies with\n the conditions stated in this License.\n\n 5. Submission of Contributions. Unless You explicitly state otherwise,\n any Contribution intentionally submitted for inclusion in the Work\n by You to the Licensor shall be under the terms and conditions of\n this License, without any additional terms or conditions.\n Notwithstanding the above, nothing herein shall supersede or modify\n the terms of any separate license agreement you may have executed\n with Licensor regarding such Contributions.\n\n 6. Trademarks. This License does not grant permission to use the trade\n names, trademarks, service marks, or product names of the Licensor,\n except as required for reasonable and customary use in describing the\n origin of the Work and reproducing the content of the NOTICE file.\n\n 7. Disclaimer of Warranty. Unless required by applicable law or\n agreed to in writing, Licensor provides the Work (and each\n Contributor provides its Contributions) on an \"AS IS\" BASIS,\n WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or\n implied, including, without limitation, any warranties or conditions\n of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A\n PARTICULAR PURPOSE. You are solely responsible for determining the\n appropriateness of using or redistributing the Work and assume any\n risks associated with Your exercise of permissions under this License.\n\n 8. Limitation of Liability. In no event and under no legal theory,\n whether in tort (including negligence), contract, or otherwise,\n unless required by applicable law (such as deliberate and grossly\n negligent acts) or agreed to in writing, shall any Contributor be\n liable to You for damages, including any direct, indirect, special,\n incidental, or consequential damages of any character arising as a\n result of this License or out of the use or inability to use the\n Work (including but not limited to damages for loss of goodwill,\n work stoppage, computer failure or malfunction, or any and all\n other commercial damages or losses), even if such Contributor\n has been advised of the possibility of such damages.\n\n 9. Accepting Warranty or Additional Liability. While redistributing\n the Work or Derivative Works thereof, You may choose to offer,\n and charge a fee for, acceptance of support, warranty, indemnity,\n or other liability obligations and/or rights consistent with this\n License. However, in accepting such obligations, You may act only\n on Your own behalf and on Your sole responsibility, not on behalf\n of any other Contributor, and only if You agree to indemnify,\n defend, and hold each Contributor harmless for any liability\n incurred by, or claims asserted against, such Contributor by reason\n of your accepting any such warranty or additional liability.\n\n END OF TERMS AND CONDITIONS\n\n APPENDIX: How to apply the Apache License to your work.\n\n To apply the Apache License to your work, attach the following\n boilerplate notice, with the fields enclosed by brackets \"[]\"\n replaced with your own identifying information. (Don't include\n the brackets!) The text should be enclosed in the appropriate\n comment syntax for the file format. We also recommend that a\n file or class name and description of purpose be included on the\n same \"printed page\" as the copyright notice for easier\n identification within third-party archives.\n\n Copyright 2019-2021 Saif Sidhik\n\n Licensed under the Apache License, Version 2.0 (the \"License\");\n you may not use this file except in compliance with the License.\n You may obtain a copy of the License at\n\n http://www.apache.org/licenses/LICENSE-2.0\n\n Unless required by applicable law or agreed to in writing, software\n distributed under the License is distributed on an \"AS IS\" BASIS,\n WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n See the License for the specific language governing permissions and\n limitations under the License." }, { "path": "NOTICE", "content": "\n franka_ros_interface package\n\n Copyright 2019-2021 Saif Sidhik\n\n Licensed under the Apache License, Version 2.0 (the \"License\");\n you may not use this file except in compliance with the License.\n You may obtain a copy of the License at\n\n http://www.apache.org/licenses/LICENSE-2.0\n\n Unless required by applicable law or agreed to in writing, software\n distributed under the License is distributed on an \"AS IS\" BASIS,\n WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n See the License for the specific language governing permissions and\n limitations under the License.\n" }, { "path": "README.md", "content": "# Franka ROS Interface [![Release](https://img.shields.io/badge/release-v0.7.1-blue.svg)](https://github.com/justagist/franka_ros_interface/tree/v0.7.1-dev) [![ROS Version](https://img.shields.io/badge/ROS-Melodic,%20Noetic-brightgreen.svg?logo=ros)](https://ros.org/) [![Python 2.7+, 3.6+](https://img.shields.io/badge/python-2.7+,%203.6+-blue.svg?logo=python)](https://www.python.org/downloads/release/python-360/)\n\n[![Codacy Badge](https://app.codacy.com/project/badge/Grade/ec16a09639d341358b73cb8cdaa57d2e)](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)\n[![Build Status](https://img.shields.io/travis/com/justagist/franka_ros_interface/v0.7.1-dev?logo=travis-ci)](https://travis-ci.com/justagist/franka_ros_interface)\n\nA 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\nproviding low-level control of the robot using ROS and [Python API][fri-doc].\n\nFranka ROS Interface provides utilites for controlling and managing the Franka Emika Panda robot. Contains exposed customisable controllers for the robot (joint position,\nvelocity, 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.\n\nDocumentation Page: \n\n**This branch requires franka_ros release version 0.7.1** [![franka_ros_version](https://img.shields.io/badge/franka_ros-v0.7.1%20release-yellow.svg)](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.)\n\nA 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.\n\n## Features\n\n- Low-level *controllers* (joint position, velocity, torque, impedance) available that can be controlled through ROS topics and Python API (including position control for gripper).\n- Real-time *robot state* (end-effector state, joint state, controller state, etc.) available through ROS topics and Python API.\n- Python API for managing controllers, coordinate frames, collision behaviour, controlling and monitoring the gripper.\n- Python API classes and utility functions to control the robot using MoveIt! and ROS Trajectory Action Service.\n- 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.\n\n**Demo Using [PandaRobot API](https://github.com/justagist/panda_robot) and [Panda Simulator][ps-repo]**\n\n ![vid](assets/panda_robot_demo.gif)\n Watch video [here](https://youtu.be/4bEVysUIvOY)\n\n ![vid](assets/panda_simulator.gif)\n Watch video [here](https://www.youtube.com/watch?v=NdSbXC0r7tU)\n\n ![vid](assets/ts_demo.gif)\n Watch video [here](https://youtu.be/a_HEmYzqEnk)\n\n## Installation\n\nThis branch works with ROS Melodic and ROS Noetic. [![Build Status](https://img.shields.io/travis/com/justagist/franka_ros_interface/v0.7.1-dev?logo=travis-ci)](https://travis-ci.com/justagist/franka_ros_interface)\n\n**NOTE:** Tested on:\n\n| ROS Version | Python Version | Franka ROS Branch |\n|-------------|----------------|-------------------------------------------------------------------------------|\n| Melodic | 2.7+ | [melodic-devel](https://github.com/frankaemika/franka_ros/tree/melodic-devel) |\n| Noetic | 3.6+ | [noetic-devel](https://github.com/frankaemika/franka_ros/tree/noetic-devel) |\n\n### Dependencies\n\n- ROS Melodic / Noetic (preferably the 'desktop-full' version)\n- *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.*\n- *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.*\n- *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))\n- [*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).\n\nOnce the above dependencies are installed, the package can be installed using catkin:\n\n```sh\n cd \n git clone -b v0.7.1-dev https://github.com/justagist/franka_ros_interface src/franka_ros_interface\n catkin build # or catkin_make (catkin build is recommended)\n source devel/setup.bash\n```\n\n*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 ..`)).*\n\nAfter building the package (this is not required if using with simulated robot):\n\n- Copy/move the *franka.sh* file to the root of the catkin_ws `$ cp src/franka_ros_interface/franka.sh ./`\n- Change the values in the copied file (described in the file).\n\n## Usage\n\n**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.**\n\n### The *franka.sh* environments\n\nOnce the values are correctly modified in the `franka.sh` file, different environments can be set for controlling the robot by sourcing this file.\n\n- 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]).\n- 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.\n- ~Simulation environment can be started by running `./franka.sh sim` (only required when using [panda_simulator][ps-repo] package).~ *Not required anymore*\n\nMore information regarding the usage of `franka.sh` can be found within the [franka.sh file](franka.sh).\n\n### Starting the Franka ROS Interface 'Driver'\n\nThe 'driver' node can be started by running (can only be used if run in 'master' environment - see [Environments](#the-frankash-environments) section above):\n\n```sh\n roslaunch franka_interface interface.launch # (use argument load_gripper:=false for starting without gripper)\n```\n\nAvailable keyword arguments for launch file:\n\n- `load_gripper`: start driver node with the Franka gripper (default: `true`).\n- `start_controllers`: load the available controllers to the controller manager (default: `true`).\n- `start_moveit`: start moveit server along with the driver node (default: `true`).\n- `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).\n\nThis starts the robot controllers and drivers to expose a variety of ROS topics and services for communicating with and controlling the robot.\n\n### Controlling and Monitoring the Robot\n\nOnce 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)).\n\nExample of the robot working with MoveIt can be found by running `roslaunch franka_moveit demo_moveit.launch`.\n\nBasic 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`).\n\nSee [documentation][fri-doc] for all available methods and functionalities.\n\nSee 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.\n\n**More usage examples can be found in the [PandaRobot](https://github.com/justagist/panda_robot) package.**\n\n#### Some useful ROS topics\n\n##### Published Topics\n\n| ROS Topic | Data |\n| ------ | ------ |\n| */franka_ros_interface/custom_franka_state_controller/robot_state* | gravity, coriolis, jacobian, cartesian velocity, etc. |\n| */franka_ros_interface/custom_franka_state_controller/tip_state* | end-effector pose, wrench, etc. |\n| */franka_ros_interface/joint_states* | joint positions, velocities, efforts |\n| */franka_ros_interface/franka_gripper/joint_states* | joint positions, velocities, efforts of gripper joints |\n\n##### Subscribed Topics\n\n| ROS Topic | Data |\n| ------ | ------ |\n| */franka_ros_interface/motion_controller/arm/joint_commands* | command the robot using the currently active controller |\n| */franka_ros_interface/franka_gripper/[move/grasp/stop/homing]* | (action msg) command the joints of the gripper |\n\nOther topics for changing the controller gains (also dynamically configurable), command timeout, etc. are also available.\n\n#### ROS Services\n\nController 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.\n\n#### Python API\n\nMost 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.\n\n[Documentation][fri-doc]\n\nMore usage examples in the [PandaRobot](https://github.com/justagist/panda_robot) package.\n\n## Related Packages\n\n- [*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.\n- [*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.\n- [*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.\n\n [ps-repo]: \n [fri-repo]: \n [fpd-repo]: \n [fri-doc]: \n [libfranka-doc]: \n [franka-ros]: \n\n## License\n\n[![License](https://img.shields.io/badge/License-Apache%202.0-blue.svg)](https://opensource.org/licenses/Apache-2.0)\n\nCopyright (c) 2019-2021, Saif Sidhik\n\nIf you use this software for research, please considering citing using [![DOI](https://zenodo.org/badge/DOI/10.5281/zenodo.3747412.svg)](https://doi.org/10.5281/zenodo.3747412).\n" }, { "path": "cmake/ClangTools.cmake", "content": "include(CMakeParseArguments)\n\nfind_program(CLANG_FORMAT_PROG clang-format-5.0 DOC \"'clang-format' executable\")\nif(CLANG_FORMAT_PROG AND NOT TARGET format)\n add_custom_target(format)\n add_custom_target(check-format)\nendif()\nfind_program(CLANG_TIDY_PROG clang-tidy-5.0 DOC \"'clang-tidy' executable\")\nif(CLANG_TIDY_PROG AND NOT TARGET tidy)\n if(NOT CMAKE_EXPORT_COMPILE_COMMANDS)\n message(WARNING \"Invoke Catkin/CMake with '-DCMAKE_EXPORT_COMPILE_COMMANDS=ON'\n to generate compilation database for 'clang-tidy'.\")\n endif()\n\n add_custom_target(tidy)\n add_custom_target(check-tidy)\nendif()\n\nfunction(add_format_target _target)\n if(NOT CLANG_FORMAT_PROG)\n return()\n endif()\n cmake_parse_arguments(ARG \"\" \"\" \"FILES\" ${ARGN})\n\n add_custom_target(format-${_target}\n COMMAND ${CLANG_FORMAT_PROG} -i ${ARG_FILES}\n WORKING_DIRECTORY ${CMAKE_CURRENT_LIST_DIR}/..\n COMMENT \"Formatting ${_target} source code with clang-format\"\n VERBATIM\n )\n add_dependencies(format format-${_target})\n\n add_custom_target(check-format-${_target}\n COMMAND scripts/format-check.sh ${CLANG_FORMAT_PROG} -output-replacements-xml ${ARG_FILES}\n WORKING_DIRECTORY ${CMAKE_CURRENT_LIST_DIR}/..\n COMMENT \"Checking ${_target} code formatting with clang-format\"\n VERBATIM\n )\n add_dependencies(check-format check-format-${_target})\nendfunction()\n\nfunction(add_tidy_target _target)\n if(NOT CLANG_TIDY_PROG)\n return()\n endif()\n cmake_parse_arguments(ARG \"\" \"\" \"FILES;DEPENDS\" ${ARGN})\n\n add_custom_target(tidy-${_target}\n COMMAND ${CLANG_TIDY_PROG} -fix -p=${CMAKE_BINARY_DIR} ${ARG_FILES}\n WORKING_DIRECTORY ${CMAKE_CURRENT_LIST_DIR}/..\n DEPENDS ${ARG_DEPENDS}\n COMMENT \"Running clang-tidy for ${_target}\"\n VERBATIM\n )\n add_dependencies(tidy tidy-${_target})\n\n add_custom_target(check-tidy-${_target}\n COMMAND scripts/fail-on-output.sh ${CLANG_TIDY_PROG} -p=${CMAKE_BINARY_DIR} ${ARG_FILES}\n WORKING_DIRECTORY ${CMAKE_CURRENT_LIST_DIR}/..\n DEPENDS ${ARG_DEPENDS}\n COMMENT \"Running clang-tidy for ${_target}\"\n VERBATIM\n )\n add_dependencies(check-tidy check-tidy-${_target})\nendfunction()\n" }, { "path": "franka.sh", "content": "#!/bin/bash\n\n# Copyright (c) 2019-2021, Saif Sidhik\n# Copyright (c) 2013-2016, Rethink Robotics\n# All rights reserved.\n\n# This is a convenient script which will set up your ROS environment and\n# should be executed with every new instance of a shell in which you plan on\n# working with Franka ROS Interface or PandaRobot.\n\n# This file is to be used in the *root* of your catkin workspace.\n\n# ****** IMPORTANT: Read \"USAGE EXPLAINED\" section below before running this script. ******\n\n\n\n# Run as './franka.sh '\n\n# ------ ARGUMENTS:\n# -- arg1 (required): select the environment for Franka ROS Interface. The available options are:\n# - 'master' / 'local' (No ): 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).\n# - '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 , this will connected to a ROS Master at that IP. If is not provided, ROS_MASTER_IP variable set below will be used.\n# - 'sim': start in simulation environment (to be used with panda_simulator package only). [Not necessary]\n\n# -- arg2 (optional):\n# - (valid only if is 'slave'/'remote'): IP address of ROS Master\n\n# eg: \n# ./franka.sh master\n# (or)\n# ./franka.sh remote [optional IP address of master]\n\n\n## ========================================= #\n## === EDIT FOLLOWING VALUES AS REQUIRED === #\n## ========================================= #\n\n# ----- EDIT THIS TO MATCH THE IP OF THE FRANKA ROBOT CONTROLLER IN THE NETWORK\nFRANKA_ROBOT_IP=\"FRANKA_ROBOT_IP.local\"\n\n# ----- EDIT THIS TO MATCH YOUR IP IN THE NETWORK\nyour_ip=\"\"\n\n# ----- EDIT THIS TO MATCH THE IP OF THE MAIN ROS MASTER PC (CONNECTED TO THE FRANKA CONTROLLER),\n# ----- ON WHICH THE Franka ROS Interface 'driver' NODES WILL BE RUNNING (SEE \"USAGE EXPLAINED\" BELOW).\n# ----- THIS VALUE IS NEEDED ONLY IF YOU WILL USE THE 'remote' ENVIRONMENT ON THIS MACHINE.\nROS_MASTER_IP=\"\"\n\nros_version=\"melodic\"\n\n## ========================================= #\n## ========== USAGE EXPLAINED ============== #\n## ========================================= #\n\n# 1. If there is only one computer controlling the robot, there is no need \n# to fill the $ROS_MASTER_IP variable in the field above. However, this\n# machine should have RT kernel set up correctly and should be able to \n# connect to the robot using libfranka. \n# For using Franka ROS Interface in this case:\n# a) Move this file to the root of the catkin ws, and fill in the \n# values for $FRANKA_ROBOT_IP and $your_ip above.\n# b) run `./franka.sh master`\n# c) Start the driver node (see Usage in README)\n# d) Once the driver node is running correctly, on a different \n# terminal, run `./franka master` again from the catkin_ws root to \n# connect to the same ROS master as the driver node.\n# e) You should now be able to communicate with the robot using \n# Franka ROS Interface from this terminal session. Use ROS \n# topics/services etc. or Franka ROS Interface Python API or scripts \n# to control and monitor the robot. (test by running `rostopic list`)\n# f) For every new terminal, make sure to run `./franka.sh master` first.\n\n# 2. If you want to use multiple machines for controlling the robot, all \n# machines should be in the same network as the robot controller.\n# AT LEAST ONE MACHINE should have RT kernel set up and should be able to \n# properly connect to the robot using libfranka. ONE of these machines \n# should run the driver node as explained in steps 1.a - 1.c above. \n# Follow steps 1.d - 1.f if Franka ROS Interface is needed againg on this \n# RT machine (which is running the driver).\n#\n# NOTE: ONLY ONE MACHINE IN THE NETWORK SHOULD BE RUNNING THE DRIVER NODE.\n#\n# Other computers in the network need not have RT kernels, but should have \n# correctly installed lifranka, franka-ros, and Franka ROS Interface. For \n# controlling the robot from these machines:\n# a) Move this file to the root of the catkin_ws of the remote machines, \n# and fill in the values for $FRANKA_ROBOT_IP, $your_ip, and \n# $ROS_MASTER_IP above. THE $ROS_MASTER_IP should correspond to the \n# IP address of the RT machine running the 'driver' node.\n# b) Then run `./franka.sh remote`. This should connect the current \n# session to the ROS master running the driver node (to test if the \n# connection is correct, run `rostopic list` to see if the published \n# and subscribed topics from Franka ROS Interface driver node is \n# accessible from the remote computer).\n# c) As long as the driver node is running on the RT machine, you should \n# now be able to communicate with the robot using Franka ROS Interface \n# from this terminal session. Use ROS topics/services etc. or Franka \n# ROS Interface Python API or scripts to control and monitor the robot.\n# d) For every new terminal in the remote machines, make sure to run \n# `./franka.sh remote` first.\n\n\n## ========================================= #\n## ========================================= #\n## === DO NOT EDIT BELOW THIS LINE ========= #\n\n\nif [ \"${1}\" == \"sim\" ]; then\n your_ip=\"127.0.0.1\"\nfi\n\nif [ \"${your_ip}\" == \"\" ]; then\n 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\"\n exit 1\nfi\n\n\n\nif [ \"${1}\" == \"master\" ] || [[ \"${1}\" == \"local\" ]]; then\n if [ \"${FRANKA_ROBOT_IP}\" == \"FRANKA_ROBOT_IP.local\" ]; then\n 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\"\n exit 1\n fi\n ROS_MASTER_IP=\"$your_ip\"\n IS_MASTER=true\nelif [ \"${1}\" == \"slave\" ] || [[ \"${1}\" == \"remote\" ]]; then\n if [[ $# -ge 2 ]]; then\n ROS_MASTER_IP=\"$2\"\n fi\n IS_MASTER=false \nelif [ \"${1}\" == \"sim\" ]; then\n ROS_MASTER_IP=\"$your_ip\"\n IS_MASTER=\"true\"\n FRANKA_ROBOT_IP=\"sim\"\nelse\n echo -e \"\\n\\t\\033[01;31m\\033[1mError:\\033[00m [franka.sh] Atleast one argument has to be passed\n \n Usage: ./franka_sdk \n\n ------ ARGUMENTS:\n -- arg1:\n - 'master' / 'local' (No ): The local machine will be the ros master. Run this on the main master pc (connected to the franka controller). \n - 'slave' / 'remote': If IP address is provided as , this will make sure that this PC is connected to the ROS Master at that IP. If provided, ROS_MASTER_IP set below will be used\n - 'sim': Franka robot simulation environment will be used. Real robot will not be controlled.\n\n -- arg2:\n - (valid only if is 'slave'/'remote'): IP address of ROS Master\n \"\n exit 1\nfi\n\ntf=$(mktemp)\ntrap \"rm -f -- '${tf}'\" EXIT\n\ntopdir=$(basename $(readlink -f $(dirname ${BASH_SOURCE[0]})))\n\ncat <<-EOF > ${tf}\n [ -s \"\\${HOME}\"/.bashrc ] && source \"\\${HOME}\"/.bashrc\n [ -s \"\\${HOME}\"/.bash_profile ] && source \"\\${HOME}\"/.bash_profile\n\n # verify this script is moved out of franka_sdk folder\n if [[ -e \"${topdir}/franka_rospy_sdk/package.xml\" ]]; then\n echo -ne \"EXITING - This script must be moved from the franka_rospy_sdk folder \\\nto the root of your catkin workspace.\\n\"\n exit 1\n fi\n\n\n # check for ros installation\n if [ ! -d \"/opt/ros\" ] || [ ! \"$(ls -A /opt/ros)\" ]; then\n echo \"EXITING - No ROS installation found in /opt/ros.\"\n echo \"Is ROS installed?\"\n exit 1\n fi\n\n # verify specified ros version is installed\n ros_setup=\"/opt/ros/${ros_version}\"\n if [ ! -d \"\\${ros_setup}\" ]; then\n echo -ne \"EXITING - Failed to find ROS \\${ros_version} installation \\\nin \\${ros_setup}.\\n\"\n exit 1\n fi\n\n # verify the ros setup.sh file exists\n if [ ! -s \"\\${ros_setup}\"/setup.sh ]; then\n echo -ne \"EXITING - Failed to find the ROS environment script: \\\n\"\\${ros_setup}\"/setup.sh.\\n\"\n exit 1\n fi\n\n # verify the user is running this script in the root of the catkin\n # workspace and that the workspace has been compiled.\n if [ ! -s \"devel/setup.bash\" ]; then\n echo -ne \"EXITING - \\n1) Please verify that this script is being run \\\nin the root of your catkin workspace.\\n2) Please verify that your workspace \\\nhas been built (source /opt/ros/\\${ros_version}/setup.sh; catkin_make).\\n\\\n3) Run this script again upon completion of your workspace build.\\n\"\n exit 1\n fi\n\n export FRANKA_ROBOT_IP=$FRANKA_ROBOT_IP\n\n [ -n \"${your_ip}\" ] && export ROS_IP=\"${your_ip}\"\n [ -n \"${ROS_MASTER_IP}\" ] && \\\n export ROS_MASTER_URI=\"http://${ROS_MASTER_IP}:11311\"\n\n\n # source the catkin setup bash script\n source devel/setup.bash\n\n\n if [[ $IS_MASTER == true ]]; then\n echo -e \"\\nROBOT: $FRANKA_ROBOT_IP\\n\"\n else\n echo -e \"\\nROS MASTER: $ROS_MASTER_IP\\n\" \n fi\n\n # setup the bash prompt\n export __ROS_PROMPT=\\${__ROS_PROMPT:-0}\n [ \\${__ROS_PROMPT} -eq 0 -a -n \"\\${PROMPT_COMMAND}\" ] && \\\n export __ORIG_PROMPT_COMMAND=\\${PROMPT_COMMAND}\n\n __ros_prompt () {\n if [ -n \"\\${__ORIG_PROMPT_COMMAND}\" ]; then\n eval \\${__ORIG_PROMPT_COMMAND}\n fi\n if ! echo \\${PS1} | grep '\\[franka' &>/dev/null; then\n if [[ $your_ip == \"localhost\" ]]; then\n export PS1=\"\\[\\033[00;33m\\][franka - Robot@\\\n${FRANKA_ROBOT_IP}]\\[\\033[00m\\] \\${PS1}\"\n elif [[ $IS_MASTER == true ]]; then\n export PS1=\"\\[\\033[00;33m\\][franka - Robot@\\\n${FRANKA_ROBOT_IP}]\\[\\033[00m\\] \\${PS1}\"\n else\n export PS1=\"\\[\\033[00;33m\\][franka - Master@\\\n${ROS_MASTER_IP}]\\[\\033[00m\\] \\${PS1}\"\n fi\n fi\n }\n\n if [ \"\\${TERM}\" != \"dumb\" ]; then\n export PROMPT_COMMAND=__ros_prompt\n __ROS_PROMPT=1\n elif ! echo \\${PS1} | grep '\\[franka' &>/dev/null; then\n if [[ $your_ip == \"localhost\" ]]; then\n export PS1=\"\\[\\033[00;33m\\][franka - Robot@\\\n${FRANKA_ROBOT_IP}]\\[\\033[00m\\] \\${PS1}\"\n elif [[ $IS_MASTER == true ]]; then\n export PS1=\"\\[\\033[00;33m\\][franka - Robot@\\\n${FRANKA_ROBOT_IP}]\\[\\033[00m\\] \\${PS1}\"\n else\n export PS1=\"\\[\\033[00;33m\\][franka - Master@\\\n${ROS_MASTER_IP}]\\[\\033[00m\\] \\${PS1}\"\n fi\n fi\n\nEOF\n\n${SHELL} --rcfile ${tf}\n\nrm -f -- \"${tf}\"\ntrap - EXIT\n" }, { "path": "franka_common/franka_core_msgs/CMakeLists.txt", "content": "cmake_minimum_required(VERSION 2.8.3)\n\nproject(franka_core_msgs)\n\nfind_package(catkin REQUIRED COMPONENTS\n message_generation\n actionlib_msgs\n std_msgs\n franka_msgs\n geometry_msgs\n sensor_msgs\n control_msgs\n)\n\nadd_message_files( DIRECTORY msg\n FILES\n JointCommand.msg\n RobotState.msg\n EndPointState.msg\n JointLimits.msg\n JointControllerStates.msg\n)\n\n# add_service_files( DIRECTORY srv\n# FILES\n# SolvePositionIK.srv\n# SolvePositionFK.srv\n# IOComponentCommandSrv.srv\n# )\n\n# add_action_files(DIRECTORY action FILES CalibrationCommand.action)\n\n## Build\ngenerate_messages(DEPENDENCIES std_msgs geometry_msgs control_msgs sensor_msgs actionlib_msgs franka_msgs)\n\ncatkin_package(CATKIN_DEPENDS message_runtime std_msgs geometry_msgs control_msgs sensor_msgs franka_msgs actionlib_msgs)\n\n\n## Install\n\n\n## Testing\n" }, { "path": "franka_common/franka_core_msgs/msg/EndPointState.msg", "content": "std_msgs/Header header\n\nfloat64[16] O_T_EE # Measured end effector pose in base frame\n\n# ----- Moved ee velocity to robot state, because it is being computed using J*dq. EE vel is not being provided natively\n#float64[6] O_dP_EE_c # Last commanded end effector twist in base frame.\n#float64[6] O_dP_EE_d # Desired end effector twist in base frame.\n#float64[6] O_ddP_EE_c # Last commanded end effector acceleration in base frame.\n\ngeometry_msgs/WrenchStamped O_F_ext_hat_K # Estimated external wrench (force, torque) acting on stiffness frame, expressed relative to the base frame\ngeometry_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)\n\n\n\n" }, { "path": "franka_common/franka_core_msgs/msg/JointCommand.msg", "content": "Header header\n\nint32 mode # Mode in which to command arm\n\nstring[] names # Joint names order for command\n\n# Fields of commands indexed according to the Joint names vector.\n# Command fields required for a desired mode are listed in the comments\nfloat64[] position # (radians) Required for POSITION_MODE and IMPEDANCE_MODE\nfloat64[] velocity # (radians/sec) Required for VELOCITY_MODE and IMPEDANCE_MODE\nfloat64[] acceleration # (radians/sec^2) Required for \nfloat64[] effort # (newton-meters) Required for TORQUE_MODE\n\n# Modes available to command arm\nint32 POSITION_MODE=1\nint32 VELOCITY_MODE=2\nint32 TORQUE_MODE=3\nint32 IMPEDANCE_MODE=4\n" }, { "path": "franka_common/franka_core_msgs/msg/JointControllerStates.msg", "content": "Header header\n\nstring controller_name\n\nstring[] names # Joint names order for command\n\ncontrol_msgs/JointControllerState[] joint_controller_states \n\n\n" }, { "path": "franka_common/franka_core_msgs/msg/JointLimits.msg", "content": "# names of the joints\nstring[] joint_names\n\n# lower bound on the angular position in radians\nfloat64[] position_lower\n\n# upper bound on the angular position in radians\nfloat64[] position_upper\n\n# symmetric maximum joint velocity in radians/second\nfloat64[] velocity\n\n# symmetric maximum joint acceleration in radians/second^2\nfloat64[] accel\n\n# symmetric maximum joint torque in Newton-meters\nfloat64[] effort\n" }, { "path": "franka_common/franka_core_msgs/msg/RobotState.msg", "content": "std_msgs/Header header\n\nfloat64[6] cartesian_collision\nfloat64[6] cartesian_contact\nfloat64[6] O_dP_EE # EE vel computed as J*dq\n\n# float64[7] q # joint position, velocity, and effort in joint_states topic\n# float64[7] dq\n\nfloat64[7] q_d\nfloat64[7] dq_d\n# float64[7] tau_J\nfloat64[7] dtau_J # torque derivative\nfloat64[7] tau_J_d # desired joint torque\n# float64[6] K_F_ext_hat_K\n# float64[2] elbow\n# float64[2] elbow_d\n\nfloat64[7] joint_collision\nfloat64[7] joint_contact\n\n# float64[6] O_F_ext_hat_K # in tip state\n\nfloat64[7] tau_ext_hat_filtered # filtered external torque\nfloat64[3] F_x_Cee # Configured center of mass of the end effector load with respect to flange frame.\nfloat64[3] F_x_Cload # Configured center of mass of the external load with respect to flange frame.\nfloat64[3] F_x_Ctotal # Combined center of mass of the end effector load and the external load with respect to flange frame.\nfloat64[9] I_ee # Configured rotational inertia matrix of the end effector load with respect to center of mass.\nfloat64[9] I_load\nfloat64[9] I_total\n\nfloat64 m_ee # configured mass of end-effector\nfloat64 m_load # configured mass of external load\nfloat64 m_total\n\n\nfloat64[7] gravity\nfloat64[7] coriolis\nfloat64[49] mass_matrix # mass matrix of end-effector wrt base frame # Vectorized 7x7, column-major\n\nfloat64[42] O_Jac_EE # zero jacobian of end-effector frame. Vectorized 6x7 Jacobian, column-major\n\n# float64[16] O_T_EE # ----- moved to endpointstate msg # Vectorized 4x4, column-major\nfloat64[16] O_T_EE_d # Last desired end effector pose of motion generation in base frame. # Vectorized 4x4, column-major\nfloat64[16] F_T_EE # End effector frame pose in flange frame # Vectorized 4x4, column-major\nfloat64[16] F_T_NE # Nominal End effector frame pose in flange frame (fixed in Desk) # Vectorized 4x4, column-major\nfloat64[16] NE_T_EE # End effector frame pose in Nominal End effector frame # Vectorized 4x4, column-major\nfloat64[16] EE_T_K # Stiffness frame pose in end effector frame # Vectorized 4x4, column-major\nfloat64 time\n\nuint8 ROBOT_MODE_OTHER=0\nuint8 ROBOT_MODE_IDLE=1\nuint8 ROBOT_MODE_MOVE=2\nuint8 ROBOT_MODE_GUIDING=3\nuint8 ROBOT_MODE_REFLEX=4\nuint8 ROBOT_MODE_USER_STOPPED=5\nuint8 ROBOT_MODE_AUTOMATIC_ERROR_RECOVERY=6\nuint8 robot_mode\n\nfranka_msgs/Errors current_errors\nfranka_msgs/Errors last_motion_errors\n" }, { "path": "franka_common/franka_core_msgs/package.xml", "content": "\n\n franka_core_msgs\n 0.7.1\n \n Messages and Services required for communication with the Franka robot when using \n franka_ros_interface and panda_simulator\n \n\n \n Saif Sidhik\n \n Apache 2.0\n\n Saif Sidhik\n\n catkin\n\n \n message_generation\n std_msgs\n geometry_msgs\n sensor_msgs\n control_msgs\n franka_msgs\n actionlib_msgs\n\n message_runtime\n std_msgs\n franka_msgs\n geometry_msgs\n control_msgs\n sensor_msgs\n actionlib_msgs\n\n\n" }, { "path": "franka_interface/CMakeLists.txt", "content": "cmake_minimum_required(VERSION 2.8.3)\nproject(franka_interface)\n\nset(CMAKE_BUILD_TYPE Release)\n\nset(CMAKE_CXX_STANDARD 14)\nset(CMAKE_CXX_STANDARD_REQUIRED ON)\n\nfind_package(catkin REQUIRED COMPONENTS\n controller_interface\n dynamic_reconfigure\n franka_hw\n franka_control\n controller_manager\n franka_core_msgs\n geometry_msgs\n hardware_interface\n pluginlib\n realtime_tools\n roscpp\n rospy\n)\n\nfind_package(Eigen3 REQUIRED)\nfind_package(Franka 0.5.0 REQUIRED)\n\n\ncatkin_package(\n LIBRARIES custom_franka_state_controller\n CATKIN_DEPENDS\n controller_interface\n franka_msgs\n dynamic_reconfigure\n franka_hw\n franka_control\n franka_core_msgs\n geometry_msgs\n hardware_interface\n pluginlib\n realtime_tools\n roscpp\n DEPENDS Franka\n)\n\nadd_library(custom_franka_state_controller\n src/robot_state_controller.cpp\n)\n\nadd_dependencies(custom_franka_state_controller\n ${${PROJECT_NAME}_EXPORTED_TARGETS}\n ${catkin_EXPORTED_TARGETS}\n)\n\ntarget_link_libraries(custom_franka_state_controller PUBLIC\n ${Franka_LIBRARIES}\n ${catkin_LIBRARIES}\n)\n\ntarget_include_directories(custom_franka_state_controller SYSTEM PUBLIC\n ${Franka_INCLUDE_DIRS}\n ${EIGEN3_INCLUDE_DIRS}\n ${catkin_INCLUDE_DIRS}\n)\ntarget_include_directories(custom_franka_state_controller PUBLIC\n include\n)\n\n## franka_control_node\nadd_executable(custom_franka_control_node\n src/franka_control_node.cpp\n src/motion_controller_interface.cpp\n)\n\nadd_dependencies(custom_franka_control_node\n ${${PROJECT_NAME}_EXPORTED_TARGETS}\n ${catkin_EXPORTED_TARGETS}\n\n)\n\ntarget_link_libraries(custom_franka_control_node\n ${Franka_LIBRARIES}\n ${franka_control_LIBRARIES}\n # franka_control_services\n ${catkin_LIBRARIES}\n)\n\ntarget_include_directories(custom_franka_control_node SYSTEM PUBLIC\n ${Franka_INCLUDE_DIRS}\n ${catkin_INCLUDE_DIRS}\n include\n)\n\n## Installation\ninstall(TARGETS custom_franka_state_controller\n custom_franka_control_node\n ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}\n LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}\n RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}\n)\ninstall(DIRECTORY launch\n DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}\n)\ninstall(DIRECTORY config\n DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}\n)\ninstall(FILES state_controller_plugin.xml\n DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}\n)\n\ninclude(${CMAKE_CURRENT_LIST_DIR}/../cmake/ClangTools.cmake OPTIONAL\n RESULT_VARIABLE CLANG_TOOLS\n)\nif(CLANG_TOOLS)\n file(GLOB_RECURSE SOURCES ${CMAKE_CURRENT_SOURCE_DIR}/src/*.cpp)\n file(GLOB_RECURSE HEADERS\n ${CMAKE_CURRENT_SOURCE_DIR}/include/*.h\n ${CMAKE_CURRENT_SOURCE_DIR}/src/*.h\n )\n add_format_target(custom_franka_state_controller FILES ${SOURCES} ${HEADERS})\n add_tidy_target(custom_franka_state_controller custom_franka_control_node\n FILES ${SOURCES}\n DEPENDS custom_franka_state_controller\n )\nendif()\n\n\ncatkin_python_setup()\ncatkin_install_python(PROGRAMS scripts/enable_robot.py \n scripts/move_to_neutral.py\n scripts/simple_gripper.py\n tests/demo_joint_positions_keyboard.py\n DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})\n# ## Tools\n# include(${CMAKE_CURRENT_LIST_DIR}/../cmake/ClangTools.cmake OPTIONAL\n# RESULT_VARIABLE CLANG_TOOLS\n# )\n# if(CLANG_TOOLS)\n# file(GLOB_RECURSE SOURCES ${CMAKE_CURRENT_SOURCE_DIR}/src/*.cpp)\n# file(GLOB_RECURSE HEADERS\n# ${CMAKE_CURRENT_SOURCE_DIR}/include/*.h\n# ${CMAKE_CURRENT_SOURCE_DIR}/src/*.h\n# )\n# add_format_target(custom_franka_state_controller FILES ${SOURCES} ${HEADERS})\n# add_tidy_target(custom_franka_state_controller\n# FILES ${SOURCES}\n# DEPENDS custom_franka_state_controller\n# )\n# endif()" }, { "path": "franka_interface/README.md", "content": "# franka_interface\n\n**See induvidual source files for more detailed documentation.**\n\n### ArmInterface\n- Interface class that can monitor and control the robot \n- Provides all required information about robot state and end-effector state\n- Joint positions, velocities, and effort can be directly controlled and monitored using available methods\n- Smooth interpolation of joint positions possible\n- End-effector and Stiffness frames can be directly set (uses FrankaFramesInterface from *franka_ros_interface/franka_tools*)\n\n### GripperInterface\n- Interface class to monitor and control gripper\n- Gripper open, close methods\n- Grasp, move joints methods \n\n### RobotEnable\n- Interface class to reset robot when in recoverable error (use *enable_robot.py* script in *scripts/*)\n\n### RobotParams\n- Collects and stores all useful information about the robot from the ROS parameter server\n\n### Scripts\n\n- Scripts providing simple gripper actions, and robot reset are provided\n" }, { "path": "franka_interface/config/basic_controllers.yaml", "content": "# load the internal joint trajectory controller, and the custom robot state controller\n\nposition_joint_trajectory_controller:\n type: position_controllers/JointTrajectoryController\n joints:\n - panda_joint1\n - panda_joint2\n - panda_joint3\n - panda_joint4\n - panda_joint5\n - panda_joint6\n - panda_joint7\n constraints:\n goal_time: 0.5\n panda_joint1:\n goal: 0.05\n panda_joint2:\n goal: 0.05\n panda_joint3:\n goal: 0.05\n panda_joint4:\n goal: 0.05\n panda_joint5:\n goal: 0.05\n panda_joint6:\n goal: 0.05\n panda_joint7:\n goal: 0.05\n\nfranka_ros_interface:\n custom_franka_state_controller:\n type: franka_interface/CustomFrankaStateController\n publish_rate: 1000 # [Hz]\n joint_names:\n - panda_joint1\n - panda_joint2\n - panda_joint3\n - panda_joint4\n - panda_joint5\n - panda_joint6\n - panda_joint7\n arm_id: panda\n" }, { "path": "franka_interface/config/robot_config.yaml", "content": "# global robot configuration and details\n\nrobot_config:\n\n joint_names:\n - panda_joint1\n - panda_joint2\n - panda_joint3\n - panda_joint4\n - panda_joint5\n - panda_joint6\n - panda_joint7\n arm_id: panda\n # Activate rate limiter? [true|false]\n joint_limit_warning_threshold: 0.1 # [rad]\n rate_limiting: true\n # Cutoff frequency of the low-pass filter. Set to >= 1000 to deactivate.\n cutoff_frequency: 100\n # Internal controller for motion generators [joint_impedance|cartesian_impedance]\n internal_controller: joint_impedance\n\n neutral_pose:\n panda_joint1: -0.017792060227770554 \n panda_joint2: -0.7601235411041661 \n panda_joint3: 0.019782607023391807 \n panda_joint4: -2.342050140544315 \n panda_joint5: 0.029840531355804868 \n panda_joint6: 1.5411935298621688 \n panda_joint7: 0.7534486589746342 \n\n joint_config:\n joint_acceleration_limit:\n panda_joint1: 15.0 # rad / sec^2\n panda_joint2: 7.5 # rad / sec^2\n panda_joint3: 10.0 # rad / sec^2\n panda_joint4: 12.5 # rad / sec^2\n panda_joint5: 15.0 # rad / sec^2\n panda_joint6: 20.0 # rad / sec^2\n panda_joint7: 20.0 # rad / sec^2\n # panda_finger_joint1: 12.0 # rad / sec^2\n # panda_finger_joint2: 12.0 # rad / sec^2\n\n joint_velocity_limit:\n panda_joint1: 2.1750 # rad / sec\n panda_joint2: 2.1750 # rad / sec\n panda_joint3: 2.1750 # rad / sec\n panda_joint4: 2.1750 # rad / sec\n panda_joint5: 2.6100 # rad / sec\n panda_joint6: 2.6100 # rad / sec\n panda_joint7: 2.6100 # rad / sec\n # panda_finger_joint1: 2.0 # rad / sec\n # panda_finger_joint2: 2.0 # rad / sec\n\n joint_position_limit:\n lower: \n panda_joint1: -2.8973 # rad\n panda_joint2: -1.7628 # rad\n panda_joint3: -2.8973 # rad\n panda_joint4: -3.0718 # rad\n panda_joint5: -2.8973 # rad\n panda_joint6: -0.0175 # rad\n panda_joint7: -2.8973 # rad\n upper:\n panda_joint1: 2.8973 # rad\n panda_joint2: 1.7628 # rad\n panda_joint3: 2.8973 # rad\n panda_joint4: -0.0698 # rad\n panda_joint5: 2.8973 # rad\n panda_joint6: 3.7525 # rad\n panda_joint7: 2.8973 # rad\n\n joint_effort_limit:\n panda_joint1: 87 # Nm\n panda_joint2: 87 # Nm\n panda_joint3: 87 # Nm\n panda_joint4: 87 # Nm\n panda_joint5: 12 # Nm\n panda_joint6: 12 # Nm\n panda_joint7: 12 # Nm \n\n realtime_config: enforce\n # Configure the initial defaults for the collision behavior reflexes.\n collision_config:\n lower_torque_thresholds_acceleration: [20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0] # [Nm]\n upper_torque_thresholds_acceleration: [20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0] # [Nm]\n lower_torque_thresholds_nominal: [20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0] # [Nm]\n upper_torque_thresholds_nominal: [20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0] # [Nm]\n lower_force_thresholds_acceleration: [20.0, 20.0, 20.0, 25.0, 25.0, 25.0] # [N, N, N, Nm, Nm, Nm]\n upper_force_thresholds_acceleration: [20.0, 20.0, 20.0, 25.0, 25.0, 25.0] # [N, N, N, Nm, Nm, Nm]\n lower_force_thresholds_nominal: [20.0, 20.0, 20.0, 25.0, 25.0, 25.0] # [N, N, N, Nm, Nm, Nm]\n upper_force_thresholds_nominal: [20.0, 20.0, 20.0, 25.0, 25.0, 25.0] # [N, N, N, Nm, Nm, Nm] \n\ngripper_config:\n joint_names:\n - panda_finger_joint1\n - panda_finger_joint2\n default_speed: 0.1 # [m/s]\n default_grasp_epsilon:\n inner: 0.005 # [m]\n outer: 0.005 # [m]\n\ncontrollers_config:\n position_controller: \"franka_ros_interface/position_joint_position_controller\"\n torque_controller: \"franka_ros_interface/effort_joint_torque_controller\"\n impedance_controller: \"franka_ros_interface/effort_joint_impedance_controller\"\n velocity_controller: \"franka_ros_interface/velocity_joint_velocity_controller\"\n trajectory_controller: \"position_joint_trajectory_controller\"\n default_controller: \"position_joint_trajectory_controller\" # for safety, always set a position controller as default\n 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" }, { "path": "franka_interface/include/franka_interface/motion_controller_interface.h", "content": "/***************************************************************************\n* Adapted from sawyer_simulator package\n\n*\n* @package: franka_interface\n* @metapackage: franka_ros_interface\n* @author: Saif Sidhik \n*\n\n**************************************************************************/\n\n/***************************************************************************\n* Copyright (c) 2019-2021, Saif Sidhik.\n* Copyright (c) 2013-2018, Rethink Robotics Inc.\n*\n* Licensed under the Apache License, Version 2.0 (the \"License\");\n* you may not use this file except in compliance with the License.\n* You may obtain a copy of the License at\n*\n* http://www.apache.org/licenses/LICENSE-2.0\n*\n* Unless required by applicable law or agreed to in writing, software\n* distributed under the License is distributed on an \"AS IS\" BASIS,\n* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n* See the License for the specific language governing permissions and\n* limitations under the License.\n**************************************************************************/\n#ifndef _FRANKA_INTERFACE____MOTION_CONTROLLER_INTERFACE_H_\n#define _FRANKA_INTERFACE____MOTION_CONTROLLER_INTERFACE_H_\n\n#include \n\n#include \n#include \n\n#include \n#include \n#include \n\n\nnamespace franka_interface {\n class MotionControllerInterface\n {\n public:\n /**\n * Initializes the controller manager.\n *\n * @param[in] root_node_handle Node handle in the controller_manager namespace.\n * @param[in] controller_manager the controller manager instance.\n */\n void init(ros::NodeHandle& nh,\n boost::shared_ptr controller_manager);\n\n private:\n // mutex for re-entrant calls to modeCommandCallback\n std::mutex mtx_;\n int current_mode_;\n\n realtime_tools::RealtimeBox< std::shared_ptr > box_timeout_length_;\n realtime_tools::RealtimeBox< std::shared_ptr > box_cmd_timeout_;\n\n ros::Timer cmd_timeout_timer_;\n\n ros::Subscriber joint_command_timeout_sub_;\n ros::Subscriber joint_command_sub_;\n boost::shared_ptr controller_manager_;\n\n std::string position_controller_name_;\n std::string velocity_controller_name_;\n std::string torque_controller_name_;\n std::string impedance_controller_name_;\n std::string trajectory_controller_name_;\n\n std::string default_controller_name_;\n std::string current_controller_name_;\n std::vector all_controllers_;\n\n std::map controller_name_to_mode_map_;\n protected:\n /**\n * Callback function to set time out to switch back to position mode. When using torque\n * mode controllers, consecutive torque commands should be sent in smaller intervals than\n * this timeout value\n *\n * @param[in] msg Float64 instance containing the desired timeout.\n */\n void jointCommandTimeoutCallback(const std_msgs::Float64 msg);\n\n /**\n * Callback function to choose the appropriate controller. This function checks the type\n * of controller requested by the client through the type field in the message, and \n * starts the controller.\n *\n * @param[in] msg JointCommandConstPtr instance containing the joint commands.\n */\n void jointCommandCallback(const franka_core_msgs::JointCommandConstPtr& msg);\n\n /**\n * Switch controller depending on the mode specified.\n *\n * @param[in] control_mode integer value representing the type of controller to use\n */\n bool switchControllers(int control_mode);\n\n /**\n * Switch controller to the initially defined default controller.\n */\n bool switchToDefaultController();\n\n /**\n * Check if the command timeout has been violated.\n *\n * @param[in] control_mode integer value representing the type of controller to use\n */\n void commandTimeoutCheck(const ros::TimerEvent& e);\n\n\n };\n}\n#endif // #ifndef _FRANKA_INTERFACE____MOTION_CONTROLLER_INTERFACE_H_\n" }, { "path": "franka_interface/include/franka_interface/robot_state_controller.h", "content": "/***************************************************************************\n* Adapted from robot_state_controller.cpp (franka_ros package)\n\n*\n* @package: franka_interface\n* @metapackage: franka_ros_interface\n* @author: Saif Sidhik \n*\n\n**************************************************************************/\n\n/***************************************************************************\n* Copyright (c) 2019-2021, Saif Sidhik\n* Copyright (c) 2017 Franka Emika GmbH\n*\n* Licensed under the Apache License, Version 2.0 (the \"License\");\n* you may not use this file except in compliance with the License.\n* You may obtain a copy of the License at\n*\n* http://www.apache.org/licenses/LICENSE-2.0\n*\n* Unless required by applicable law or agreed to in writing, software\n* distributed under the License is distributed on an \"AS IS\" BASIS,\n* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n* See the License for the specific language governing permissions and\n* limitations under the License.\n**************************************************************************/\n\n\n#pragma once\n\n#include \n#include \n#include \n\n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n\nnamespace franka_interface {\n\n/**\n * Custom controller to publish the robot state as ROS topics.\n */\nclass CustomFrankaStateController\n : public controller_interface::MultiInterfaceController {\n public:\n /**\n * Creates an instance of a CustomFrankaStateController.\n */\n CustomFrankaStateController() = default;\n\n /**\n * Initializes the controller with interfaces and publishers.\n *\n * @param[in] robot_hardware RobotHW instance to get a franka_hw::FrankaStateInterface from.\n * @param[in] root_node_handle Node handle in the controller_manager namespace.\n * @param[in] controller_node_handle Node handle in the controller namespace.\n */\n bool init(hardware_interface::RobotHW* robot_hardware,\n ros::NodeHandle& root_node_handle,\n ros::NodeHandle& controller_node_handle) override;\n\n /**\n * Reads the current robot state from the franka_hw::FrankaStateInterface and publishes it.\n *\n * @param[in] time Current ROS time.\n * @param[in] period Time since the last update.\n */\n void update(const ros::Time& time, const ros::Duration& period) override;\n\n private:\n void publishFrankaState(const ros::Time& time);\n void publishJointStates(const ros::Time& time);\n void publishTransforms(const ros::Time& time);\n void publishEndPointState(const ros::Time& time);\n\n std::string arm_id_;\n\n franka_hw::FrankaStateInterface* franka_state_interface_{};\n std::unique_ptr franka_state_handle_{};\n std::unique_ptr model_handle_;\n\n realtime_tools::RealtimePublisher publisher_transforms_;\n realtime_tools::RealtimePublisher publisher_franka_state_;\n realtime_tools::RealtimePublisher publisher_joint_states_;\n realtime_tools::RealtimePublisher publisher_joint_states_desired_;\n realtime_tools::RealtimePublisher publisher_tip_state_;\n franka_hw::TriggerRate trigger_publish_;\n franka::RobotState robot_state_;\n uint64_t sequence_number_ = 0;\n std::vector joint_names_;\n};\n\n} // namespace franka_interface\n" }, { "path": "franka_interface/launch/interface.launch", "content": "\n\n \n \n \n \n \n \n \n\n \n \n \n\n \n \n \n\n \n \n \n \n \n \n \n \n\n \n \n \n \n \n \n \n \n [franka_ros_interface/custom_franka_state_controller/joint_states, franka_gripper/joint_states] \n [franka_ros_interface/custom_franka_state_controller/joint_states] \n \n \n \n [franka_ros_interface/custom_franka_state_controller/joint_states_desired, franka_gripper/joint_states] \n [franka_ros_interface/custom_franka_state_controller/joint_states_desired] \n \n \n \n\n \n\n \n \n \n \n\n \n \n \n \n \n \n \n \n \n \n \n \n\n\n\n\n\n" }, { "path": "franka_interface/package.xml", "content": "\n>\n franka_interface\n 0.7.1\n \n ROS/Python interface classes for control of\n Franka Panda robot through franka-ros and libfranka.\n \n\n \n Saif Sidhik\n \n Apache 2.0\n\n Saif Sidhik\n\n catkin\n\n message_generation\n eigen\n franka_control\n\n controller_interface\n dynamic_reconfigure\n franka_hw\n controller_manager\n franka_core_msgs\n franka_msgs\n geometry_msgs\n hardware_interface\n libfranka\n pluginlib\n realtime_tools\n roscpp\n\n franka_control\n franka_description\n panda_moveit_config\n franka_moveit\n rospy\n\n\n \n \n \n\n" }, { "path": "franka_interface/scripts/enable_robot.py", "content": "#!/usr/bin/env python\n\n# /***************************************************************************\n\n# \n# @package: franka_interface\n# @metapackage: franka_ros_interface\n# @author: Saif Sidhik \n# \n\n# **************************************************************************/\n\n# /***************************************************************************\n# Copyright (c) 2019-2021, Saif Sidhik\n \n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n\n# http://www.apache.org/licenses/LICENSE-2.0\n\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n# **************************************************************************/\n\n\n\"\"\"\n\n:info: \n Utility script for enabling, disabling and checking status of robot.\n Usage:\n enable_robot.py \n\n :Args:\n : -s (--state) / -e (--enable) / -d (--disable) / -r (--reset) / -S (--stop) \n\n Not implemented: Disabling robot.\n Todo: Change reset request to service request instead of publishing message to topic.\n\n\"\"\"\n\nimport argparse\nimport sys\n\nimport rospy\n\nimport franka_interface\n\ndef main():\n rospy.init_node('panda_robot_enable')\n parser = argparse.ArgumentParser()\n parser.add_argument('-s', '--state', const='state',\n dest='actions', action='append_const',\n help='Print current robot state')\n parser.add_argument('-e', '--enable', const='enable',\n dest='actions', action='append_const',\n help='Enable the robot')\n parser.add_argument('-d', '--disable', const='disable',\n dest='actions', action='append_const',\n help='Disable the robot')\n parser.add_argument('-r', '--reset', const='reset',\n dest='actions', action='append_const',\n help='Reset the robot')\n parser.add_argument('-S', '--stop', const='stop',\n dest='actions', action='append_const',\n help='Stop the robot')\n args = parser.parse_args(rospy.myargv()[1:])\n\n if args.actions == None:\n parser.print_usage()\n parser.exit(0, \"No action defined\\n\")\n\n rs = franka_interface.RobotEnable()\n try:\n for act in args.actions:\n if act == 'state':\n print(rs.state())\n elif act == 'enable':\n rs.enable()\n elif act == 'disable':\n rs.disable()\n elif act == 'reset':\n rs.reset()\n elif act == 'stop':\n rs.stop()\n except Exception as e:\n rospy.logerr(e)\n\n return 0\n\nif __name__ == '__main__':\n sys.exit(main())\n" }, { "path": "franka_interface/scripts/move_to_neutral.py", "content": "#!/usr/bin/env python\n\n# /***************************************************************************\n\n# \n# @package: franka_interface\n# @metapackage: franka_ros_interface\n# @author: Saif Sidhik \n# \n\n# **************************************************************************/\n\n# /***************************************************************************\n# Copyright (c) 2019-2021, Saif Sidhik\n \n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n\n# http://www.apache.org/licenses/LICENSE-2.0\n\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n# **************************************************************************/\n\n\n\"\"\"\n:info: \n commands robot to move to neutral pose\n\n\"\"\"\n\nimport rospy\nfrom franka_interface import ArmInterface\n\nif __name__ == '__main__':\n rospy.init_node(\"move_to_neutral_node\")\n r = ArmInterface()\n r.move_to_neutral()" }, { "path": "franka_interface/scripts/simple_gripper.py", "content": "#!/usr/bin/env python\n\n# /***************************************************************************\n\n# \n# @package: franka_interface\n# @metapackage: franka_ros_interface\n# @author: Saif Sidhik \n# \n\n# **************************************************************************/\n\n# /***************************************************************************\n# Copyright (c) 2019-2021, Saif Sidhik\n \n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n\n# http://www.apache.org/licenses/LICENSE-2.0\n\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n# **************************************************************************/\n\n\n\"\"\"\n:info: \n NOTE: Running this script without any arguments will stop the current command being \n sent to the gripper. Use this to stop any dangerous action being run\n Utility script for simple gripper motion.\n Usage:\n python simple_gripper.py \n\n @Args:\n : Stop current gripper action\n -o (--open) : optional argument to make the gripper release and go to max width \n -c (--close) : optional argument to close the gripper \n\n\"\"\"\n\n\nimport rospy\nimport argparse\nimport actionlib\n\nfrom franka_interface import GripperInterface\nfrom franka_gripper.msg import ( StopAction, StopGoal )\n\ndef _active_cb():\n rospy.loginfo(\"GripperInterface: '{}' request active.\".format(_caller))\n\ndef _feedback_cb(msg):\n rospy.loginfo(\"GripperInterface: '{}' request feedback: \\n\\t{}\".format(_caller,msg))\n\ndef _done_cb(status, result):\n rospy.loginfo(\"GripperInterface: '{}' complete. Result: \\n\\t{}\".format(_caller, result))\n\nif __name__ == '__main__':\n \n rospy.init_node(\"gripper_stop_node\")\n\n _caller = \"stop_gripper\"\n stop_action_client = actionlib.SimpleActionClient(\"/franka_gripper/stop\", StopAction)\n stop_action_client.wait_for_server()\n\n stop_action_client.send_goal(StopGoal(), done_cb =_done_cb, active_cb = _active_cb, feedback_cb = _feedback_cb)\n\n result = stop_action_client.wait_for_result(rospy.Duration(15.))\n\n parser = argparse.ArgumentParser(\"Stop current gripper action; Open or close gripper.\")\n\n gripper_group = parser.add_mutually_exclusive_group(required=False)\n gripper_group.add_argument(\"-o\", \"--open\", dest=\"open\",\n action='store_true', default=False, help=\"open gripper\")\n gripper_group.add_argument(\"-c\", \"--close\", dest=\"close\",\n action='store_true', default=False, help=\"close gripper\")\n args = parser.parse_args(rospy.myargv()[1:])\n\n if args.open:\n gi = GripperInterface()\n gi.open()\n else:\n gi = GripperInterface()\n gi.close()\n" }, { "path": "franka_interface/setup.py", "content": "#!/usr/bin/env python\nfrom distutils.core import setup\nfrom catkin_pkg.python_setup import generate_distutils_setup\n\nd = generate_distutils_setup()\nd['packages'] = ['franka_interface','franka_dataflow']\nd['package_dir'] = {'': 'src'}\n\nsetup(**d)\n" }, { "path": "franka_interface/src/franka_control_node.cpp", "content": "// /***************************************************************************\n// * Adapted from franka_control_node.cpp (frank_ros package)\n\n// *\n// * @package: franka_interface\n// * @metapackage: franka_ros_interface\n// * @author: Saif Sidhik \n// *\n\n// **************************************************************************/\n\n// /***************************************************************************\n// * Copyright (c) 2019-2021, Saif Sidhik.\n// * Copyright (c) 2017 Franka Emika GmbH\n// *\n// * Licensed under the Apache License, Version 2.0 (the \"License\");\n// * you may not use this file except in compliance with the License.\n// * You may obtain a copy of the License at\n// *\n// * http://www.apache.org/licenses/LICENSE-2.0\n// *\n// * Unless required by applicable law or agreed to in writing, software\n// * distributed under the License is distributed on an \"AS IS\" BASIS,\n// * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// * See the License for the specific language governing permissions and\n// * limitations under the License.\n// **************************************************************************/\n\n#include \n#include \n\n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n\n#include \n\nusing franka_hw::ServiceContainer;\n\nint main(int argc, char** argv) {\n ros::init(argc, argv, \"franka_control_node\");\n\n ros::NodeHandle public_node_handle;\n ros::NodeHandle config_node_handle(\"/robot_config\");\n ros::NodeHandle base_node_handle(\"~\");\n\n franka_hw::FrankaHW franka_control;\n if (!franka_control.init(public_node_handle, config_node_handle)) {\n ROS_ERROR(\"franka_control_node: Failed to initialize FrankaHW class. Shutting down!\");\n return 1;\n }\n\n franka::Robot& robot = franka_control.robot();\n\n std::atomic_bool has_error(false);\n\n ServiceContainer services;\n services\n .advertiseService(base_node_handle, \"/franka_ros_interface/franka_control/set_joint_impedance\",\n [&robot](auto&& req, auto&& res) {\n return franka_hw::setJointImpedance(\n robot, req, res);\n })\n .advertiseService(\n base_node_handle, \"/franka_ros_interface/franka_control/set_cartesian_impedance\",\n [&robot](auto&& req, auto&& res) {\n return franka_hw::setCartesianImpedance(robot, req, res);\n })\n .advertiseService(\n base_node_handle, \"/franka_ros_interface/franka_control/set_EE_frame\",\n [&robot](auto&& req, auto&& res) { return franka_hw::setEEFrame(robot, req, res); })\n .advertiseService(\n base_node_handle, \"/franka_ros_interface/franka_control/set_K_frame\",\n [&robot](auto&& req, auto&& res) { return franka_hw::setKFrame(robot, req, res); })\n .advertiseService(\n base_node_handle, \"/franka_ros_interface/franka_control/set_force_torque_collision_behavior\",\n [&robot](auto&& req, auto&& res) {\n return franka_hw::setForceTorqueCollisionBehavior(robot, req, res);\n })\n .advertiseService(\n base_node_handle, \"/franka_ros_interface/franka_control/set_full_collision_behavior\",\n [&robot](auto&& req, auto&& res) {\n return franka_hw::setFullCollisionBehavior(robot, req, res);\n })\n .advertiseService(\n base_node_handle, \"/franka_ros_interface/franka_control/set_load\",\n [&robot](auto&& req, auto&& res) { return franka_hw::setLoad(robot, req, res); });\n\n actionlib::SimpleActionServer recovery_action_server(\n base_node_handle, \"/franka_ros_interface/franka_control/error_recovery\",\n [&](const franka_msgs::ErrorRecoveryGoalConstPtr&) {\n try {\n robot.automaticErrorRecovery();\n has_error = false;\n recovery_action_server.setSucceeded();\n ROS_INFO(\"Recovered from error\");\n } catch (const franka::Exception& ex) {\n recovery_action_server.setAborted(franka_msgs::ErrorRecoveryResult(), ex.what());\n }\n },\n false);\n franka_control.update(robot.readOnce());\n\n boost::shared_ptr control_manager;\n\n control_manager.reset(new controller_manager::ControllerManager(&franka_control, public_node_handle)); \n\n franka_interface::MotionControllerInterface motion_controller_interface_;\n motion_controller_interface_.init(public_node_handle, control_manager);\n\n recovery_action_server.start();\n\n // // Start background threads for message handling\n ros::AsyncSpinner spinner(4);\n spinner.start();\n\n while (ros::ok()) {\n ros::Time last_time = ros::Time::now();\n\n // Wait until controller has been activated or error has been recovered\n while (!franka_control.controllerActive() || has_error) {\n franka_control.update(robot.readOnce());\n\n ros::Time now = ros::Time::now();\n control_manager->update(now, now - last_time);\n franka_control.checkJointLimits();\n last_time = now;\n\n if (!ros::ok()) {\n return 0;\n }\n }\n\n try {\n // Run control loop. Will exit if the controller is switched.\n franka_control.control([&](const ros::Time& now, const ros::Duration& period) {\n if (period.toSec() == 0.0) {\n // Reset controllers before starting a motion\n control_manager->update(now, period, true);\n franka_control.checkJointLimits();\n franka_control.reset();\n } else {\n control_manager->update(now, period);\n franka_control.checkJointLimits();\n franka_control.enforceLimits(period);\n }\n return ros::ok();\n });\n } catch (const franka::ControlException& e) {\n ROS_ERROR(\"%s\", e.what());\n has_error = true;\n }\n }\n\n return 0;\n\n}" }, { "path": "franka_interface/src/franka_dataflow/__init__.py", "content": "# /***************************************************************************\n# Copyright (c) 2019-2021, Saif Sidhik\n# Copyright (c) 2013-2018, Rethink Robotics Inc.\n \n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n\n# http://www.apache.org/licenses/LICENSE-2.0\n\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n# **************************************************************************/\nfrom .wait_for import wait_for" }, { "path": "franka_interface/src/franka_dataflow/getch.py", "content": "# Copyright (c) 2013-2018, Rethink Robotics Inc.\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n#\n# http://www.apache.org/licenses/LICENSE-2.0\n#\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n\nimport sys\nimport termios\nimport tty\nfrom select import select\n\n\ndef getch(timeout=0.01):\n \"\"\"\n Retrieves a character from stdin.\n\n Returns None if no character is available within the timeout.\n Blocks if timeout < 0.\n \"\"\"\n # If this is being piped to, ignore non-blocking functionality\n if not sys.stdin.isatty():\n return sys.stdin.read(1)\n fileno = sys.stdin.fileno()\n old_settings = termios.tcgetattr(fileno)\n ch = None\n try:\n tty.setraw(fileno)\n rlist = [fileno]\n if timeout >= 0:\n [rlist, _, _] = select(rlist, [], [], timeout)\n if fileno in rlist:\n ch = sys.stdin.read(1)\n except Exception as ex:\n print(\"getch\", ex)\n raise OSError\n finally:\n termios.tcsetattr(fileno, termios.TCSADRAIN, old_settings)\n return ch\n" }, { "path": "franka_interface/src/franka_dataflow/wait_for.py", "content": "# /***************************************************************************\n# Modified from: Rethink Robotics Intera SDK\n# \n# @package: franka_interface\n# @metapackage: franka_ros_interface\n# @author: Saif Sidhik \n# \n\n# **************************************************************************/\n\n# /***************************************************************************\n# Copyright (c) 2019-2021, Saif Sidhik\n# Copyright (c) 2013-2018, Rethink Robotics Inc.\n \n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n\n# http://www.apache.org/licenses/LICENSE-2.0\n\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n# **************************************************************************/\n\n\nimport errno\n\nimport rospy\n\n\ndef wait_for(test, timeout=1.0, raise_on_error=True, rate=100,\n timeout_msg=\"timeout expired\", body=None):\n \"\"\"\n Waits until some condition evaluates to true.\n\n :param test: zero param function to be evaluated\n :param timeout: max amount of time to wait. negative/inf for indefinitely\n :param raise_on_error: raise or just return False\n :param rate: the rate at which to check\n :param timout_msg: message to supply to the timeout exception\n :param body: optional function to execute while waiting\n \"\"\"\n end_time = rospy.get_time() + timeout\n rate = rospy.Rate(rate)\n notimeout = (timeout < 0.0) or timeout == float(\"inf\")\n while not test():\n if rospy.is_shutdown():\n if raise_on_error:\n raise OSError(errno.ESHUTDOWN, \"ROS Shutdown\")\n return False\n elif (not notimeout) and (rospy.get_time() >= end_time):\n if raise_on_error:\n raise OSError(errno.ETIMEDOUT, timeout_msg)\n rospy.loginfo(timeout_msg)\n return False\n if callable(body):\n body()\n rate.sleep()\n return True\n" }, { "path": "franka_interface/src/franka_interface/__init__.py", "content": "# /***************************************************************************\n# Copyright (c) 2019-2021, Saif Sidhik\n \n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n\n# http://www.apache.org/licenses/LICENSE-2.0\n\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n# **************************************************************************/\n\nfrom .robot_params import RobotParams\nfrom .arm import ArmInterface\nfrom .gripper import GripperInterface\nfrom .robot_enable import RobotEnable\n" }, { "path": "franka_interface/src/franka_interface/arm.py", "content": "# /***************************************************************************\n\n#\n# @package: franka_interface\n# @metapackage: franka_ros_interface\n# @author: Saif Sidhik \n#\n\n# **************************************************************************/\n\n# /***************************************************************************\n# Copyright (c) 2019-2021, Saif Sidhik\n\n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n\n# http://www.apache.org/licenses/LICENSE-2.0\n\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n# **************************************************************************/\n\n\"\"\"\n:info: \n Inteface Class for Franka robot arm.\n\n\"\"\"\n\nfrom builtins import dict # for python2&3 efficient compatibility\nfrom future.utils import iteritems # for python2&3 efficient compatibility\n\nimport enum\nimport rospy\nimport warnings\nimport quaternion\nimport numpy as np\nfrom copy import deepcopy\nfrom rospy_message_converter import message_converter\n\nfrom franka_core_msgs.msg import JointCommand\nfrom franka_core_msgs.msg import RobotState, EndPointState\nfrom sensor_msgs.msg import JointState\nfrom std_msgs.msg import Float64\n\nimport franka_dataflow\nfrom .robot_params import RobotParams\n\nfrom franka_moveit import PandaMoveGroupInterface\nfrom franka_moveit.utils import create_pose_msg\nfrom franka_tools import FrankaFramesInterface, FrankaControllerManagerInterface, JointTrajectoryActionClient, CollisionBehaviourInterface\n\n\nclass TipState():\n\n def __init__(self, timestamp, pose, vel, O_effort, K_effort):\n self.timestamp = timestamp\n self._pose = pose\n self._velocity = vel\n self._effort = O_effort\n self._effort_in_K_frame = K_effort\n\n @property\n def pose(self):\n return self._pose\n\n @property\n def velocity(self):\n return self._velocity\n\n @property\n def effort(self):\n return self._effort\n\n @property\n def effort_in_K_frame(self):\n return self._effort_in_K_frame\n\n\nclass ArmInterface(object):\n\n \"\"\" \n Interface Class for an arm of Franka Panda robot.\n Constructor.\n\n :type synchronous_pub: bool\n :param synchronous_pub: designates the JointCommand Publisher\n as Synchronous if True and Asynchronous if False.\n\n Synchronous Publishing means that all joint_commands publishing to\n the robot's joints will block until the message has been serialized\n into a buffer and that buffer has been written to the transport\n of every current Subscriber. This yields predicable and consistent\n timing of messages being delivered from this Publisher. However,\n when using this mode, it is possible for a blocking Subscriber to\n prevent the joint_command functions from exiting. Unless you need exact\n JointCommand timing, default to Asynchronous Publishing (False).\n \"\"\"\n\n # Containers\n @enum.unique\n class RobotMode(enum.IntEnum):\n \"\"\"\n Enum class for specifying and retrieving the current robot mode.\n \"\"\"\n # ----- access using parameters name or value\n # ----- eg. RobotMode(0).name & RobotMode(0).value\n # ----- or RobotMode['ROBOT_MODE_OTHER'].name & RobotMode['ROBOT_MODE_OTHER'].value\n\n ROBOT_MODE_OTHER = 0\n ROBOT_MODE_IDLE = 1\n ROBOT_MODE_MOVE = 2\n ROBOT_MODE_GUIDING = 3\n ROBOT_MODE_REFLEX = 4\n ROBOT_MODE_USER_STOPPED = 5\n ROBOT_MODE_AUTOMATIC_ERROR_RECOVERY = 6\n\n def __init__(self, synchronous_pub=False):\n\n self._params = RobotParams()\n\n self._ns = self._params.get_base_namespace()\n\n self._joint_limits = self._params.get_joint_limits()\n\n joint_names = self._joint_limits.joint_names\n if not joint_names:\n rospy.logerr(\"{}: Cannot detect joint names for arm on this \"\n \"robot. Exiting Arm.init().\".format(self.__class__.__name__))\n\n return\n\n self._joint_names = joint_names\n self.name = self._params.get_robot_name()\n self._joint_angle = dict()\n self._joint_velocity = dict()\n self._joint_effort = dict()\n self._cartesian_pose = dict()\n self._cartesian_velocity = dict()\n self._cartesian_effort = dict()\n self._stiffness_frame_effort = dict()\n self._errors = dict()\n self._collision_state = False\n self._tip_states = None\n self._jacobian = None\n self._cartesian_contact = None\n\n self._robot_mode = False\n\n self._command_msg = JointCommand()\n\n # neutral pose joint positions\n self._neutral_pose_joints = self._params.get_neutral_pose()\n\n self._frames_interface = FrankaFramesInterface()\n try:\n self._collision_behaviour_interface = CollisionBehaviourInterface()\n except rospy.ROSException:\n rospy.loginfo(\"{}: Collision Service Not found. It will not be possible to change collision behaviour of robot!\".format(\n self.__class__.__name__))\n self._collision_behaviour_interface = None\n self._ctrl_manager = FrankaControllerManagerInterface(\n ns=self._ns, sim=self._params._in_sim)\n\n self._speed_ratio = 0.15\n self._F_T_NE = np.eye(1).flatten().tolist()\n self._NE_T_EE = np.eye(1).flatten().tolist()\n\n queue_size = None if synchronous_pub else 1\n with warnings.catch_warnings():\n warnings.simplefilter(\"ignore\")\n self._joint_command_publisher = rospy.Publisher(\n self._ns + '/motion_controller/arm/joint_commands',\n JointCommand,\n tcp_nodelay=True,\n queue_size=queue_size)\n\n self._pub_joint_cmd_timeout = rospy.Publisher(\n self._ns + '/motion_controller/arm/joint_command_timeout',\n Float64,\n latch=True,\n queue_size=10)\n\n self._robot_state_subscriber = rospy.Subscriber(\n self._ns + '/custom_franka_state_controller/robot_state',\n RobotState,\n self._on_robot_state,\n queue_size=1,\n tcp_nodelay=True)\n\n joint_state_topic = self._ns + '/custom_franka_state_controller/joint_states'\n self._joint_state_sub = rospy.Subscriber(\n joint_state_topic,\n JointState,\n self._on_joint_states,\n queue_size=1,\n tcp_nodelay=True)\n\n self._cartesian_state_sub = rospy.Subscriber(\n self._ns + '/custom_franka_state_controller/tip_state',\n EndPointState,\n self._on_endpoint_state,\n queue_size=1,\n tcp_nodelay=True)\n\n rospy.on_shutdown(self._clean_shutdown)\n\n err_msg = (\"%s arm init failed to get current joint_states \"\n \"from %s\") % (self.name.capitalize(), joint_state_topic)\n franka_dataflow.wait_for(lambda: len(list(self._joint_angle.keys())) > 0,\n timeout_msg=err_msg, timeout=5.0)\n\n err_msg = (\"%s arm, init failed to get current tip_state \"\n \"from %s\") % (self.name.capitalize(), self._ns + 'tip_state')\n franka_dataflow.wait_for(lambda: len(list(self._cartesian_pose.keys())) > 0,\n timeout_msg=err_msg, timeout=5.0)\n\n err_msg = (\"%s arm, init failed to get current robot_state \"\n \"from %s\") % (self.name.capitalize(), self._ns + 'robot_state')\n franka_dataflow.wait_for(lambda: self._jacobian is not None,\n timeout_msg=err_msg, timeout=5.0)\n\n # 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.\n try:\n self._movegroup_interface = PandaMoveGroupInterface(\n use_panda_hand_link=True if self._params._in_sim else False)\n except:\n rospy.loginfo(\"{}: MoveGroup was not found! This is okay if moveit service is not required!\".format(\n self.__class__.__name__))\n self._movegroup_interface = None\n\n self.set_joint_position_speed(self._speed_ratio)\n\n def _clean_shutdown(self):\n self._joint_state_sub.unregister()\n self._cartesian_state_sub.unregister()\n self._pub_joint_cmd_timeout.unregister()\n self._robot_state_subscriber.unregister()\n self._joint_command_publisher.unregister()\n\n def get_movegroup_interface(self):\n \"\"\"\n :return: the movegroup interface instance associated with the robot.\n :rtype: franka_moveit.PandaMoveGroupInterface\n \"\"\"\n return self._movegroup_interface\n\n def get_robot_params(self):\n \"\"\"\n :return: Useful parameters from the ROS parameter server.\n :rtype: franka_interface.RobotParams\n \"\"\"\n return self._params\n\n def get_joint_limits(self):\n \"\"\"\n Return the joint limits (defined in the parameter server)\n\n :rtype: franka_core_msgs.msg.JointLimits\n :return: JointLimits\n \"\"\"\n return self._joint_limits\n\n def joint_names(self):\n \"\"\"\n Return the names of the joints for the specified limb.\n\n :rtype: [str]\n :return: ordered list of joint names from proximal to distal (i.e. shoulder to wrist).\n \"\"\"\n return self._joint_names\n\n def _on_joint_states(self, msg):\n\n for idx, name in enumerate(msg.name):\n if name in self._joint_names:\n self._joint_angle[name] = msg.position[idx]\n self._joint_velocity[name] = msg.velocity[idx]\n self._joint_effort[name] = msg.effort[idx]\n\n def _on_robot_state(self, msg):\n\n self._robot_mode = self.RobotMode(msg.robot_mode)\n\n self._robot_mode_ok = (self._robot_mode.value != self.RobotMode.ROBOT_MODE_REFLEX) and (\n self._robot_mode.value != self.RobotMode.ROBOT_MODE_USER_STOPPED)\n\n self._jacobian = np.asarray(msg.O_Jac_EE).reshape(6, 7, order='F')\n\n self._cartesian_velocity = {\n 'linear': np.asarray([msg.O_dP_EE[0], msg.O_dP_EE[1], msg.O_dP_EE[2]]),\n 'angular': np.asarray([msg.O_dP_EE[3], msg.O_dP_EE[4], msg.O_dP_EE[5]])}\n\n self._cartesian_contact = msg.cartesian_contact\n self._cartesian_collision = msg.cartesian_collision\n\n self._joint_contact = msg.joint_contact\n self._joint_collision = msg.joint_collision\n\n if not self._params._in_sim:\n self._F_T_NE = msg.F_T_NE # should be constant normally\n self._NE_T_EE = msg.NE_T_EE\n self._F_T_EE = msg.F_T_EE\n if self._frames_interface:\n self._frames_interface._update_frame_data(\n self._NE_T_EE, msg.EE_T_K)\n\n self._joint_inertia = np.asarray(\n msg.mass_matrix).reshape(7, 7, order='F')\n\n self.q_d = msg.q_d\n self.dq_d = msg.dq_d\n\n self._gravity = np.asarray(msg.gravity)\n self._coriolis = np.asarray(msg.coriolis)\n\n self._errors = message_converter.convert_ros_message_to_dictionary(\n msg.current_errors)\n\n def coriolis_comp(self):\n \"\"\"\n Return coriolis compensation torques. Useful for compensating coriolis when\n performing direct torque control of the robot.\n\n :rtype: np.ndarray\n :return: 7D joint torques compensating for coriolis.\n \"\"\"\n return self._coriolis\n\n def gravity_comp(self):\n \"\"\"\n Return gravity compensation torques.\n\n :rtype: np.ndarray\n :return: 7D joint torques compensating for gravity.\n \"\"\"\n return self._gravity\n\n def get_robot_status(self):\n \"\"\"\n Return dict with all robot status information.\n\n :rtype: dict\n :return: ['robot_mode' (RobotMode object), 'robot_status' (bool), 'errors' (dict() of errors and their truth value), 'error_in_curr_status' (bool)]\n \"\"\"\n return {'robot_mode': self._robot_mode, 'robot_status': self._robot_mode_ok, 'errors': self._errors, 'error_in_current_state': self.error_in_current_state()}\n\n def in_safe_state(self):\n \"\"\"\n Return True if the specified limb is in safe state (no collision, reflex, errors etc.).\n\n :rtype: bool\n :return: True if the arm is in safe state, False otherwise.\n \"\"\"\n return self._robot_mode_ok and not self.error_in_current_state()\n\n def error_in_current_state(self):\n \"\"\"\n Return True if the specified limb has experienced an error.\n\n :rtype: bool\n :return: True if the arm has error, False otherwise.\n \"\"\"\n return not all([e == False for e in list(self._errors.values())])\n\n def what_errors(self):\n \"\"\"\n Return list of error messages if there is error in robot state\n\n :rtype: [str]\n :return: list of names of current errors in robot state\n \"\"\"\n return [e for e in self._errors if self._errors[e] == True] if self.error_in_current_state() else None\n\n def _on_endpoint_state(self, msg):\n\n cart_pose_trans_mat = np.asarray(msg.O_T_EE).reshape(4, 4, order='F')\n\n self._cartesian_pose = {\n 'position': cart_pose_trans_mat[:3, 3],\n 'orientation': quaternion.from_rotation_matrix(cart_pose_trans_mat[:3, :3]),\n 'ori_mat': cart_pose_trans_mat[:3,:3]}\n\n self._cartesian_effort = {\n 'force': np.asarray([msg.O_F_ext_hat_K.wrench.force.x,\n msg.O_F_ext_hat_K.wrench.force.y,\n msg.O_F_ext_hat_K.wrench.force.z]),\n\n 'torque': np.asarray([msg.O_F_ext_hat_K.wrench.torque.x,\n msg.O_F_ext_hat_K.wrench.torque.y,\n msg.O_F_ext_hat_K.wrench.torque.z])\n }\n\n self._stiffness_frame_effort = {\n 'force': np.asarray([msg.K_F_ext_hat_K.wrench.force.x,\n msg.K_F_ext_hat_K.wrench.force.y,\n msg.K_F_ext_hat_K.wrench.force.z]),\n\n 'torque': np.asarray([msg.K_F_ext_hat_K.wrench.torque.x,\n msg.K_F_ext_hat_K.wrench.torque.y,\n msg.K_F_ext_hat_K.wrench.torque.z])\n }\n\n self._tip_states = TipState(msg.header.stamp, deepcopy(self._cartesian_pose), deepcopy(\n self._cartesian_velocity), deepcopy(self._cartesian_effort), deepcopy(self._stiffness_frame_effort))\n\n def joint_angle(self, joint):\n \"\"\"\n Return the requested joint angle.\n\n :type joint: str\n :param joint: name of a joint\n :rtype: float\n :return: angle in radians of individual joint\n \"\"\"\n return self._joint_angle[joint]\n\n def joint_angles(self):\n \"\"\"\n Return all joint angles.\n\n :rtype: dict({str:float})\n :return: unordered dict of joint name Keys to angle (rad) Values\n \"\"\"\n return deepcopy(self._joint_angle)\n\n def joint_ordered_angles(self):\n \"\"\"\n Return all joint angles.\n\n :rtype: [float]\n :return: joint angles (rad) orded by joint_names from proximal to distal (i.e. shoulder to wrist).\n \"\"\"\n return [self._joint_angle[name] for name in self._joint_names]\n\n def joint_velocity(self, joint):\n \"\"\"\n Return the requested joint velocity.\n\n :type joint: str\n :param joint: name of a joint\n :rtype: float\n :return: velocity in radians/s of individual joint\n \"\"\"\n return self._joint_velocity[joint]\n\n def joint_velocities(self):\n \"\"\"\n Return all joint velocities.\n\n :rtype: dict({str:float})\n :return: unordered dict of joint name Keys to velocity (rad/s) Values\n \"\"\"\n return deepcopy(self._joint_velocity)\n\n def joint_effort(self, joint):\n \"\"\"\n Return the requested joint effort.\n\n :type joint: str\n :param joint: name of a joint\n :rtype: float\n :return: effort in Nm of individual joint\n \"\"\"\n return self._joint_effort[joint]\n\n def joint_efforts(self):\n \"\"\"\n Return all joint efforts.\n\n :rtype: dict({str:float})\n :return: unordered dict of joint name Keys to effort (Nm) Values\n \"\"\"\n return deepcopy(self._joint_effort)\n\n def endpoint_pose(self):\n \"\"\"\n Return Cartesian endpoint pose {position, orientation}.\n\n :rtype: dict({str:np.ndarray (shape:(3,)), str:quaternion.quaternion})\n :return: position and orientation as named tuples in a dict\n\n - 'position': np.array of x, y, z\n - 'orientation': quaternion x,y,z,w in quaternion format\n\n \"\"\"\n return deepcopy(self._cartesian_pose)\n\n def endpoint_velocity(self):\n \"\"\"\n Return Cartesian endpoint twist {linear, angular}.\n\n :rtype: dict({str:np.ndarray (shape:(3,)),str:np.ndarray (shape:(3,))})\n :return: linear and angular velocities as named tuples in a dict\n\n - 'linear': np.array of x, y, z\n - 'angular': np.array of x, y, z (angular velocity along the axes)\n \"\"\"\n return deepcopy(self._cartesian_velocity)\n\n def endpoint_effort(self, in_base_frame=True):\n \"\"\"\n Return Cartesian endpoint wrench {force, torque}.\n\n :param in_base_frame: if True, returns end-effector effort with respect to base frame, else in stiffness frame [default: True]\n :type in_base_frame: bool\n :rtype: dict({str:np.ndarray (shape:(3,)),str:np.ndarray (shape:(3,))})\n :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)\n\n - 'force': Cartesian force on x,y,z axes in np.ndarray format\n - 'torque': Torque around x,y,z axes in np.ndarray format\n \"\"\"\n return deepcopy(self._cartesian_effort) if in_base_frame else deepcopy(self._stiffness_frame_effort)\n\n def exit_control_mode(self, timeout=5, velocity_tolerance=1e-2):\n \"\"\"\n Clean exit from advanced control modes (joint torque or velocity).\n Resets control to joint position mode with current positions until \n further advanced control commands are sent to the robot.\n\n .. note:: In normal cases, this method is not required as the\n interface automatically switches to position control mode if\n advanced control commands (velocity/torque) are not sent at \n regular intervals. Therefore it is enough to stop sending the \n commands to disable advanced control modes.\n\n .. note:: In sim, this method does nothing.\n\n :type timeout: float\n :param timeout: seconds to wait for robot to stop moving before giving up [default: 5]\n :type velocity_tolerance: float\n :param velocity_tolerance: tolerance \n \"\"\"\n if self._params._in_sim: return\n\n self.set_command_timeout(0.05)\n rospy.sleep(0.5)\n\n def check_stop():\n return np.allclose(np.asarray(list(self._joint_velocity.values())), 0., atol=velocity_tolerance)\n\n rospy.loginfo(\"{}: Waiting for robot to stop moving to exit control mode...\".format(\n self.__class__.__name__))\n franka_dataflow.wait_for(\n test=lambda: check_stop(),\n timeout=timeout,\n timeout_msg=\"{}: FAILED to exit control mode! The robot may be still moving. Controllers might not switch correctly\".format(\n self.__class__.__name__),\n rate=20,\n raise_on_error=False\n )\n\n rospy.loginfo(\"{}: Done. Setting position control target to current position.\".format(\n self.__class__.__name__))\n self.set_joint_positions(self.joint_angles())\n\n def tip_states(self):\n \"\"\"\n Return Cartesian endpoint state for a given tip name\n\n :rtype: TipState object\n :return: pose, velocity, effort, effort_in_K_frame\n \"\"\"\n return deepcopy(self._tip_states)\n\n def joint_inertia_matrix(self):\n \"\"\"\n Returns the current joint inertia matrix given by libfranka.\n\n :return: joint inertia matrix (7,7)\n :rtype: np.ndarray [7x7]\n \"\"\"\n return deepcopy(self._joint_inertia)\n\n def zero_jacobian(self):\n \"\"\"\n Returns the current jacobian matrix given by libfranka.\n \n :return: end-effector jacobian (6,7)\n :rtype: np.ndarray [6x7]\n \"\"\"\n return deepcopy(self._jacobian)\n\n def set_command_timeout(self, timeout):\n \"\"\"\n Set the timeout in seconds for the joint controller. Advanced\n control commands (vel, torque) should be sent within this\n time, or else the controller will switch back to default\n control mode.\n\n :type timeout: float\n :param timeout: timeout in seconds\n \"\"\"\n self._pub_joint_cmd_timeout.publish(Float64(timeout))\n\n def set_joint_position_speed(self, speed=0.3):\n \"\"\"\n Set ratio of max joint speed to use during joint position \n moves (only for move_to_joint_positions).\n\n Set the proportion of maximum controllable velocity to use\n during joint position control execution. The default ratio\n is `0.3`, and can be set anywhere from [0.0-1.0] (clipped).\n Once set, a speed ratio will persist until a new execution\n speed is set.\n\n :type speed: float\n :param speed: ratio of maximum joint speed for execution\n default= 0.3; range= [0.0-1.0]\n \"\"\"\n if speed > 0.3:\n rospy.logwarn(\"{}: Setting speed above 0.3 could be risky!! Be extremely careful.\".format(\n self.__class__.__name__))\n if self._movegroup_interface:\n self._movegroup_interface.set_velocity_scale(speed * 2)\n self._speed_ratio = speed\n\n def set_joint_positions(self, positions):\n \"\"\"\n Commands the joints of this limb to the specified positions.\n\n :type positions: dict({str:float}\n :param positions: dict of {'joint_name':joint_position,}\n \"\"\"\n self._command_msg.names = self._joint_names\n self._command_msg.position = [positions[j] for j in self._joint_names]\n self._command_msg.mode = JointCommand.POSITION_MODE\n self._command_msg.header.stamp = rospy.Time.now()\n self._joint_command_publisher.publish(self._command_msg)\n\n def set_joint_velocities(self, velocities):\n \"\"\"\n Commands the joints of this limb to the specified velocities.\n\n :type velocities: dict({str:float})\n :param velocities: dict of {'joint_name':joint_velocity,}\n \"\"\"\n self._command_msg.names = self._joint_names\n self._command_msg.velocity = [velocities[j] for j in self._joint_names]\n self._command_msg.mode = JointCommand.VELOCITY_MODE\n self._command_msg.header.stamp = rospy.Time.now()\n self._joint_command_publisher.publish(self._command_msg)\n\n def set_joint_torques(self, torques):\n \"\"\"\n Commands the joints of this limb with the specified torques.\n\n :type torques: dict({str:float})\n :param torques: dict of {'joint_name':joint_torque,}\n \"\"\"\n self._command_msg.names = self._joint_names\n self._command_msg.effort = [torques[j] for j in self._joint_names]\n self._command_msg.mode = JointCommand.TORQUE_MODE\n self._command_msg.header.stamp = rospy.Time.now()\n self._joint_command_publisher.publish(self._command_msg)\n\n def set_joint_positions_velocities(self, positions, velocities):\n \"\"\"\n Commands the joints of this limb using specified positions and velocities using impedance control.\n Command at time t is computed as:\n\n :math:`u_t= coriolis\\_factor * coriolis\\_t + K\\_p * (positions - curr\\_positions) + K\\_d * (velocities - curr\\_velocities)`\n\n\n :type positions: [float]\n :param positions: desired joint positions as an ordered list corresponding to joints given by self.joint_names()\n :type velocities: [float]\n :param velocities: desired joint velocities as an ordered list corresponding to joints given by self.joint_names()\n \"\"\"\n self._command_msg.names = self._joint_names\n self._command_msg.position = positions\n self._command_msg.velocity = velocities\n self._command_msg.mode = JointCommand.IMPEDANCE_MODE\n self._command_msg.header.stamp = rospy.Time.now()\n self._joint_command_publisher.publish(self._command_msg)\n\n def has_collided(self):\n \"\"\"\n Returns true if either joint collision or cartesian collision is detected. \n Collision thresholds can be set using instance of :py:class:`franka_tools.CollisionBehaviourInterface`.\n \"\"\"\n return any(self._joint_collision) or any(self._cartesian_collision)\n\n def move_to_neutral(self, timeout=15.0, speed=0.15):\n \"\"\"\n Command the Limb joints to a predefined set of \"neutral\" joint angles.\n From rosparam /franka_control/neutral_pose.\n\n :type timeout: float\n :param timeout: seconds to wait for move to finish [15]\n :type speed: float\n :param speed: ratio of maximum joint speed for execution\n default= 0.15; range= [0.0-1.0]\n \"\"\"\n self.set_joint_position_speed(speed)\n self.move_to_joint_positions(self._neutral_pose_joints, timeout)\n\n def move_to_joint_positions(self, positions, timeout=10.0,\n threshold=0.00085,\n test=None, use_moveit=True):\n \"\"\"\n (Blocking) Commands the limb to the provided positions.\n Waits until the reported joint state matches that specified.\n This function uses a low-pass filter using JointTrajectoryService \n to smooth the movement or optionally uses MoveIt! to plan and \n execute a trajectory.\n\n :type positions: dict({str:float})\n :param positions: joint_name:angle command\n :type timeout: float\n :param timeout: seconds to wait for move to finish [15]\n :type threshold: float\n :param threshold: position threshold in radians across each joint when\n move is considered successful [0.00085]\n :param test: optional function returning True if motion must be aborted\n :type use_moveit: bool\n :param use_moveit: if set to True, and movegroup interface is available, \n move to the joint positions using moveit planner.\n \"\"\"\n\n curr_controller = self._ctrl_manager.set_motion_controller(\n self._ctrl_manager.joint_trajectory_controller)\n\n if use_moveit and self._movegroup_interface:\n self._movegroup_interface.go_to_joint_positions(\n [positions[n] for n in self._joint_names], tolerance=threshold)\n\n else:\n if use_moveit:\n rospy.logwarn(\"{}: MoveGroupInterface was not found! Using JointTrajectoryActionClient instead.\".format(\n self.__class__.__name__))\n min_traj_dur = 0.5\n traj_client = JointTrajectoryActionClient(\n joint_names=self.joint_names())\n traj_client.clear()\n\n dur = []\n for j in range(len(self._joint_names)):\n dur.append(max(abs(positions[self._joint_names[j]] - self._joint_angle[self._joint_names[j]]\n ) / self._joint_limits.velocity[j], min_traj_dur))\n\n traj_client.add_point(\n positions=[self._joint_angle[n] for n in self._joint_names], time=0.0001)\n traj_client.add_point(positions=[\n positions[n] for n in self._joint_names], time=max(dur)/self._speed_ratio)\n\n def genf(joint, angle):\n def joint_diff():\n return abs(angle - self._joint_angle[joint])\n return joint_diff\n\n diffs = [genf(j, a) for j, a in list(iteritems(positions)) if\n j in self._joint_angle]\n\n fail_msg = \"{}: {} limb failed to reach commanded joint positions.\".format(\n self.__class__.__name__, self.name.capitalize())\n\n def test_collision():\n if self.has_collided():\n rospy.logerr(' '.join([\"Collision detected.\", fail_msg]))\n return True\n return False\n\n traj_client.start() # send the trajectory action request\n # traj_client.wait(timeout = timeout)\n\n franka_dataflow.wait_for(\n test=lambda: test_collision() or traj_client.result() is not None or\n (callable(test) and test() == True) or\n (all(diff() < threshold for diff in diffs)),\n timeout=timeout,\n timeout_msg=fail_msg,\n rate=100,\n raise_on_error=False\n )\n res = traj_client.result()\n if res is not None and res.error_code:\n rospy.loginfo(\"Trajectory Server Message: {}\".format(res))\n\n rospy.sleep(0.5)\n\n self._ctrl_manager.set_motion_controller(curr_controller)\n\n rospy.loginfo(\"{}: Trajectory controlling complete\".format(\n self.__class__.__name__))\n \n def get_flange_pose(self, pos=None, ori=None):\n \"\"\"\n Get the pose of flange (panda_link8) given the pose of the end-effector frame.\n\n .. note:: In sim, this method does nothing.\n\n :param pos: position of the end-effector frame in the robot's base frame, defaults to current end-effector position\n :type pos: np.ndarray, optional\n :param ori: orientation of the end-effector frame, defaults to current end-effector orientation\n :type ori: quaternion.quaternion, optional\n :return: corresponding flange frame pose in the robot's base frame\n :rtype: np.ndarray, quaternion.quaternion\n \"\"\"\n if pos is None:\n pos = self._cartesian_pose['position']\n \n if ori is None:\n ori = self._cartesian_pose['orientation']\n\n if self._params._in_sim:\n return pos, ori\n \n # get corresponding flange frame pose using transformation matrix\n F_T_EE = np.asarray(self._F_T_EE).reshape(4, 4, order=\"F\")\n mat = quaternion.as_rotation_matrix(ori)\n\n new_ori = mat.dot(F_T_EE[:3,:3].T)\n new_pos = pos - new_ori.dot(F_T_EE[:3, 3])\n\n return new_pos, quaternion.from_rotation_matrix(new_ori)\n\n def move_to_cartesian_pose(self, pos, ori=None, use_moveit=True):\n \"\"\"\n Move robot end-effector to specified cartesian pose using MoveIt! (also avoids obstacles if they are defined using :py:class:`franka_moveit.ExtendedPlanningSceneInterface`)\n\n :param pos: target end-effector position\n :type pos: [float] or np.ndarray\n :param ori: target orientation quaternion for end-effector, defaults to current ori\n :type ori: quaternion.quaternion or [float] (quaternion in w,x,y,z order), optional\n :param use_moveit: Flag for using MoveIt (redundant for now; only works if set to True), defaults to True\n :type use_moveit: bool, optional\n \"\"\"\n if not use_moveit or self._movegroup_interface is None:\n rospy.logerr(\"{}: MoveGroupInterface was not found! Aborting cartesian planning.\".format(\n self.__class__.__name__))\n return\n\n if ori is None:\n ori = self._cartesian_pose['orientation']\n self.get_flange_pose(pos, ori)\n curr_controller = self._ctrl_manager.set_motion_controller(\n self._ctrl_manager.joint_trajectory_controller)\n\n ## == Plan avoids defined scene obstacles ==\n self._movegroup_interface.go_to_cartesian_pose(\n create_pose_msg(*self.get_flange_pose(pos, ori)))\n\n ## =========================================\n\n ## == does not avoid scene obstacles ==\n # plan, _ = self._movegroup_interface.plan_cartesian_path(\n # [create_pose_msg(pos, ori)])\n # self._movegroup_interface.execute_plan(plan)\n\n ## ====================================\n\n rospy.sleep(0.5)\n self._ctrl_manager.set_motion_controller(curr_controller)\n rospy.loginfo(\"{}: Trajectory controlling complete\".format(\n self.__class__.__name__))\n\n def pause_controllers_and_do(self, func, *args, **kwargs):\n \"\"\"\n Temporarily stops all active controllers and calls the provided function\n before restarting the previously active controllers.\n\n :param func: the function to be called\n :type func: callable\n \"\"\"\n assert callable(\n func), \"{}: Invalid argument provided to ArmInterface->pause_controllers_and_do. Argument 1 should be callable.\".format(self.__class__.__name__)\n active_controllers = self._ctrl_manager.list_active_controllers(\n only_motion_controllers=True)\n\n rospy.loginfo(\"{}: Stopping motion controllers temporarily...\".format(\n self.__class__.__name__))\n for ctrlr in active_controllers:\n self._ctrl_manager.stop_controller(ctrlr.name)\n rospy.sleep(1.)\n\n retval = func(*args, **kwargs)\n\n rospy.sleep(1.)\n rospy.loginfo(\"{}: Restarting previously active motion controllers.\".format(\n self.__class__.__name__))\n for ctrlr in active_controllers:\n self._ctrl_manager.start_controller(ctrlr.name)\n rospy.sleep(1.)\n rospy.loginfo(\"{}: Controllers restarted.\".format(\n self.__class__.__name__))\n\n return retval\n\n def reset_EE_frame(self):\n \"\"\"\n Reset EE frame to default. (defined by \n FrankaFramesInterface.DEFAULT_TRANSFORMATIONS.EE_FRAME \n global variable defined in :py:class:`franka_tools.FrankaFramesInterface` \n source code). By default, this resets to align with the nominal-end effector\n frame (F_T_NE) in the flange frame.\n\n :rtype: [bool, str]\n :return: [success status of service request, error msg if any]\n \"\"\"\n if self._frames_interface:\n\n if self._frames_interface.EE_frame_is_reset():\n rospy.loginfo(\"{}: EE Frame already reset\".format(\n self.__class__.__name__))\n return True\n\n return self.pause_controllers_and_do(self._frames_interface.reset_EE_frame)\n\n else:\n rospy.logwarn(\"{}: Frames changing not available in simulated environment\".format(\n self.__class__.__name__))\n return False\n\n def set_EE_frame(self, frame):\n \"\"\"\n Set new EE frame based on the transformation given by 'frame', which is the \n transformation matrix defining the new desired EE frame with respect to the \n nominal end-effector frame (NE_T_EE).\n Motion controllers are stopped and restarted for switching.\n\n :type frame: [float (16,)] / np.ndarray (4x4) \n :param frame: transformation matrix of new EE frame wrt nominal end-effector frame (column major)\n :rtype: [bool, str]\n :return: [success status of service request, error msg if any]\n \"\"\"\n if self._frames_interface:\n\n if self._frames_interface.frames_are_same(self._frames_interface.get_EE_frame(as_mat=True), frame):\n rospy.loginfo(\"{}: EE Frame already at the target frame.\".format(\n self.__class__.__name__))\n return True\n\n return self.pause_controllers_and_do(self._frames_interface.set_EE_frame, frame)\n\n else:\n rospy.logwarn(\"{}: Frames changing not available in simulated environment\".format(\n self.__class__.__name__))\n return False\n\n def set_EE_at_frame(self, frame_name, timeout=5.0):\n \"\"\"\n Set new EE frame to the same frame as the link frame given by 'frame_name'.\n Motion controllers are stopped and restarted for switching\n\n :type frame_name: str \n :param frame_name: desired tf frame name in the tf tree\n :rtype: [bool, str]\n :return: [success status of service request, error msg if any]\n \"\"\"\n if self._frames_interface:\n if not self._frames_interface.EE_frame_already_set(self._frames_interface.get_link_tf(frame_name)):\n\n return self.pause_controllers_and_do(self._frames_interface.set_EE_at_frame, frame_name=frame_name, timeout=timeout)\n\n else:\n rospy.logwarn(\"{}: Frames changing not available in simulated environment\".format(\n self.__class__.__name__))\n return False\n\n def set_collision_threshold(self, cartesian_forces=None, joint_torques=None):\n \"\"\"\n Set Force Torque thresholds for deciding robot has collided.\n\n :return: True if service call successful, False otherwise\n :rtype: bool\n :param cartesian_forces: Cartesian force threshold for collision detection [x,y,z,R,P,Y] (robot motion stops if violated)\n :type cartesian_forces: [float] size 6\n :param joint_torques: Joint torque threshold for collision (robot motion stops if violated)\n :type joint_torques: [float] size 7\n \"\"\"\n if self._collision_behaviour_interface:\n return self._collision_behaviour_interface.set_collision_threshold(joint_torques=joint_torques, cartesian_forces=cartesian_forces)\n else:\n rospy.logwarn(\"No CollisionBehaviourInterface object found!\")\n\n def get_controller_manager(self):\n \"\"\"\n :return: the FrankaControllerManagerInterface instance associated with the robot.\n :rtype: franka_tools.FrankaControllerManagerInterface\n \"\"\"\n return self._ctrl_manager\n\n def get_frames_interface(self):\n \"\"\"\n :return: the FrankaFramesInterface instance associated with the robot.\n :rtype: franka_tools.FrankaFramesInterface\n \"\"\"\n return self._frames_interface\n\n\nif __name__ == '__main__':\n rospy.init_node('test_fri')\n r = Arm()\n\n # rate = rospy.Rate(10)\n # while not rospy.is_shutdown():\n # rate.sleep()\n" }, { "path": "franka_interface/src/franka_interface/gripper.py", "content": "# /***************************************************************************\n\n# \n# @package: franka_interface\n# @metapackage: franka_ros_interface\n# @author: Saif Sidhik \n# \n\n# **************************************************************************/\n\n# /***************************************************************************\n# Copyright (c) 2019-2021, Saif Sidhik\n \n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n\n# http://www.apache.org/licenses/LICENSE-2.0\n\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n# **************************************************************************/\n\n\"\"\"\n:info: \n Inteface Class for Franka gripper.\n\n\"\"\"\nfrom builtins import dict # for python2&3 efficient compatibility\n\nimport rospy\nimport actionlib\nimport franka_dataflow\nfrom copy import deepcopy\nfrom sensor_msgs.msg import JointState\n\nfrom franka_gripper.msg import ( GraspAction, GraspGoal, \n HomingAction, HomingGoal, \n MoveAction, MoveGoal,\n StopAction, StopGoal,\n GraspEpsilon )\n\nclass GripperInterface(object):\n \"\"\"\n Interface class for the gripper on the Franka Panda robot.\n\n\n :param gripper_joint_names: Names of the finger joints\n :param ns: base namespace of interface ('frank_ros_interface'/'panda_simulator')\n :param calibrate: Attempts to calibrate the gripper when initializing class (defaults True)\n\n :type calibrate: bool\n :type gripper_joint_names: [str]\n :type ns: str\n\n \"\"\"\n\n def __init__(self, gripper_joint_names = ('panda_finger_joint1', 'panda_finger_joint2'), calibrate = False, **kwargs):\n \"\"\"\n Constructor.\n \"\"\"\n \n self.name = '/franka_gripper'\n\n ns = self.name +'/'\n\n self._joint_positions = dict()\n self._joint_names = gripper_joint_names\n self._joint_velocity = dict()\n self._joint_effort = dict()\n\n self._joint_states_state_sub = rospy.Subscriber(ns + 'joint_states', JointState, self._joint_states_callback, queue_size = 1, tcp_nodelay = True)\n\n self._exists = False\n\n # ----- Initial test to see if gripper is loaded\n try:\n rospy.get_param(\"/franka_gripper/robot_ip\")\n except KeyError:\n rospy.loginfo(\"FrankaGripper: could not detect gripper.\")\n return\n except (socket.error, socket.gaierror):\n print (\"Failed to connect to the ROS parameter server!\\n\"\n \"Please check to make sure your ROS networking is \"\n \"properly configured:\\n\")\n sys.exit()\n\n # ----- Wait for the gripper device status to be true\n 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 ):\n return\n self._exists = True\n\n self._gripper_speed = 0.05\n\n self._homing_action_client = actionlib.SimpleActionClient(\"{}homing\".format(ns), HomingAction)\n\n self._grasp_action_client = actionlib.SimpleActionClient(\"{}grasp\".format(ns), GraspAction)\n\n self._move_action_client = actionlib.SimpleActionClient(\"{}move\".format(ns), MoveAction)\n\n self._stop_action_client = actionlib.SimpleActionClient(\"{}stop\".format(ns), StopAction)\n\n\n rospy.loginfo(\"GripperInterface: Waiting for gripper action servers... \")\n self._homing_action_client.wait_for_server()\n self._grasp_action_client.wait_for_server()\n self._move_action_client.wait_for_server()\n self._stop_action_client.wait_for_server()\n rospy.loginfo(\"GripperInterface: Gripper action servers found! \")\n\n self.MIN_FORCE = 0.01\n self.MAX_FORCE = 50 # documentation says upto 70N is possible as continuous force (max upto 140N)\n\n self.MIN_WIDTH = 0.0001\n self.MAX_WIDTH = 0.2\n\n if calibrate:\n self.calibrate()\n\n\n\n @property\n def exists(self):\n \"\"\"\n Check if a gripper was identified as connected to the robot.\n\n :return: True if gripper was detected, False otherwise\n :rtype: bool\n \"\"\"\n return self._exists\n\n def set_velocity(self, value):\n \"\"\"\n Set default value for gripper joint motions. Used for move and grasp commands.\n \n :param value: speed value [m/s]\n :type value: float\n \n \"\"\"\n 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)\n self._gripper_speed = value \n\n\n def _joint_states_callback(self, msg):\n\n for idx, name in enumerate(msg.name):\n if name in self._joint_names:\n self._joint_positions[name] = msg.position[idx]\n self._joint_velocity[name] = msg.velocity[idx]\n self._joint_effort[name] = msg.effort[idx]\n\n def joint_names(self):\n \"\"\"\n Return the names of the joints for the specified limb.\n\n :rtype: [str]\n :return: ordered list of joint names.\n \"\"\"\n return self._joint_names\n\n def joint_position(self, joint):\n \"\"\"\n Return the requested joint position.\n\n :param joint: name of a joint\n :type joint: str\n\n :rtype: float\n :return: position individual joint\n \"\"\"\n return self._joint_positions[joint]\n\n def joint_positions(self):\n \"\"\"\n Return all joint positions.\n\n :rtype: dict({str:float})\n :return: unordered dict of joint name Keys to pos\n \"\"\"\n return deepcopy(self._joint_positions)\n\n def joint_ordered_positions(self):\n \"\"\"\n Return all joint positions.\n\n :rtype: [double]\n :return: joint positions ordered by joint_names.\n \"\"\"\n return [self._joint_positions[name] for name in self._joint_names]\n\n def joint_velocity(self, joint):\n \"\"\"\n Return the requested joint velocity.\n\n :param joint: name of a joint\n :type joint: str\n\n :rtype: float\n :return: velocity in radians/s of individual joint\n \"\"\"\n return self._joint_velocity[joint]\n\n def joint_velocities(self):\n \"\"\"\n Return all joint velocities.\n\n :rtype: dict({str:float})\n :return: unordered dict of joint name Keys to velocity (rad/s) Values\n \"\"\"\n return deepcopy(self._joint_velocity)\n\n def joint_ordered_velocities(self):\n \"\"\"\n Return all joint velocities.\n\n :rtype: [double]\n :return: joint velocities ordered by joint_names.\n \"\"\"\n return [self._joint_velocity[name] for name in self._joint_names]\n\n\n def joint_effort(self, joint):\n \"\"\"\n Return the requested joint effort.\n\n :param joint: name of a joint\n :type joint: str\n\n :rtype: float\n :return: effort in Nm of individual joint\n \"\"\"\n return self._joint_effort[joint]\n\n def joint_efforts(self):\n \"\"\"\n Return all joint efforts.\n\n :rtype: dict({str:float})\n :return: unordered dict of joint name Keys to effort (Nm) Values\n \"\"\"\n return deepcopy(self._joint_effort)\n\n def joint_ordered_efforts(self):\n \"\"\"\n Return all joint efforts.\n\n :rtype: [double]\n :return: joint efforts ordered by joint_names.\n \"\"\"\n return [self._joint_effort[name] for name in self._joint_names]\n\n def _active_cb(self):\n rospy.logdebug(\"GripperInterface: '{}' request active.\".format(self._caller))\n\n def _feedback_cb(self, msg):\n rospy.logdebug(\"GripperInterface: '{}' request feedback: \\n\\t{}\".format(self._caller,msg))\n\n def _done_cb(self, status, result):\n rospy.logdebug(\"GripperInterface: '{}' complete. Result: \\n\\t{}\".format(self._caller, result))\n\n\n def home_joints(self, wait_for_result = False):\n \"\"\"\n Performs homing of the gripper.\n \n After changing the gripper fingers, a homing needs to be done.\n This is needed to estimate the maximum grasping width.\n\n :param wait_for_result: if True, this method will block till response is \n recieved from server\n :type wait_for_result: bool\n \n :return: success\n :rtype: bool \n \n \"\"\"\n self._caller = \"home_joints\"\n\n goal = HomingGoal()\n\n self._homing_action_client.send_goal(goal, done_cb =self._done_cb, active_cb = self._active_cb, feedback_cb = self._feedback_cb)\n\n if wait_for_result:\n result = self._homing_action_client.wait_for_result(rospy.Duration(15.))\n return result\n\n return True\n\n def open(self):\n \"\"\"\n Open gripper to max possible width.\n\n :return: True if command was successful, False otherwise.\n :rtype: bool\n \"\"\"\n self._caller = \"open gripper\"\n return self.move_joints(0.2)\n\n def close(self):\n \"\"\"\n close gripper to till collision is detected.\n\n .. note:: This method is known to have a bug where it sometimes opens\n the gripper when it is already closed. Use :py:meth:`close` with\n argument 0 (zero) if you want to close it, or use :py:meth:`grasp`\n wherever possible.\n \n .. note:: This is not exactly doing what it should. The behaviour is \n faked by catching the error thrown when trying to grasp a very small\n object with a very small force. Since the gripper will actually hit the\n object before it reaches the commanded width, we catch the feedback \n and send the gripper stop command to stop it where it is.\n\n :return: True if command was successful, False otherwise.\n :rtype: bool\n \"\"\"\n def cb( _, result):\n if not result.success:\n self.stop_action()\n self._caller = \"close gripper\"\n return self.grasp(0.0, 0.1, cb = cb)\n\n def calibrate(self):\n return self.home_joints(wait_for_result = True)\n\n def move_joints(self, width, speed = None, wait_for_result = True):\n \"\"\"\n Moves the gripper fingers to a specified width.\n \n :param width: Intended opening width. [m]\n :param speed: Closing speed. [m/s]\n :param wait_for_result: if True, this method will block till response is \n recieved from server\n\n :type width: float\n :type speed: float\n :type wait_for_result: bool\n \n :return: True if command was successful, False otherwise.\n :rtype: bool\n \"\"\"\n self._caller = \"move_joints\"\n\n goal = MoveGoal()\n if not speed:\n speed = self._gripper_speed\n goal.width = width\n goal.speed = speed\n\n self._move_action_client.send_goal(goal, done_cb =self._done_cb, active_cb = self._active_cb, feedback_cb = self._feedback_cb)\n\n if wait_for_result:\n result = self._move_action_client.wait_for_result(rospy.Duration(15.))\n return result\n\n return True\n\n\n def stop_action(self):\n \"\"\"\n Stops a currently running gripper move or grasp.\n \n :return: True if command was successful, False otherwise.\n :rtype: bool\n \"\"\"\n self._caller = \"stop_action\"\n\n goal = StopGoal()\n\n self._stop_action_client.send_goal(goal, done_cb =self._done_cb, active_cb = self._active_cb, feedback_cb = self._feedback_cb)\n\n result = self._stop_action_client.wait_for_result(rospy.Duration(15.))\n return result\n\n def grasp(self, width, force, speed = None, epsilon_inner = 0.005, epsilon_outer = 0.005,wait_for_result = True, cb = None):\n \"\"\"\n Grasps an object.\n \n An object is considered grasped if the distance :math:`d` between the gripper fingers satisfies\n :math:`(width - epsilon\\_inner) < d < (width + epsilon\\_outer)`.\n \n :param width: Size of the object to grasp. [m]\n :param speed: Closing speed. [m/s]\n :param force: Grasping force. [N]\n :param epsilon_inner: Maximum tolerated deviation when the actual grasped width is smaller\n than the commanded grasp width.\n :param epsilon_outer: Maximum tolerated deviation when the actual grasped width is wider\n than the commanded grasp width.\n :param cb: Optional callback function to use when the service call is done\n\n :type width: float\n :type speed: float\n :type force: float\n :type epsilon_inner: float\n :type epsilon_outer: float\n\n :return: True if an object has been grasped, false otherwise.\n :rtype: bool\n \"\"\"\n self._caller = \"grasp_action\"\n\n if not speed:\n speed = self._gripper_speed\n\n goal = GraspGoal()\n goal.width = width\n goal.speed = speed\n goal.force = force\n goal.epsilon = GraspEpsilon(inner = epsilon_inner, outer = epsilon_outer)\n\n if not cb:\n cb = self._done_cb\n\n self._grasp_action_client.send_goal(goal, done_cb = cb, active_cb = self._active_cb, feedback_cb = self._feedback_cb)\n\n if wait_for_result:\n result = self._grasp_action_client.wait_for_result(rospy.Duration(15.))\n return result\n\n return True\n\n" }, { "path": "franka_interface/src/franka_interface/robot_enable.py", "content": "# /***************************************************************************\n\n# \n# @package: franka_interface\n# @metapackage: franka_ros_interface\n# @author: Saif Sidhik \n# \n\n# **************************************************************************/\n\n# /***************************************************************************\n# Copyright (c) 2019-2021, Saif Sidhik\n \n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n\n# http://www.apache.org/licenses/LICENSE-2.0\n\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n# **************************************************************************/\n\n\"\"\"\n:info: \n Wrapper class for enabling and resetting robot state.\n\n\"\"\"\n\n\nimport rospy\nfrom threading import Lock\n\nfrom franka_msgs.msg import ErrorRecoveryActionGoal\nfrom franka_core_msgs.msg import RobotState\n\nimport franka_dataflow\nfrom .robot_params import RobotParams\n\nclass RobotEnable(object):\n \"\"\"\n Class RobotEnable - simple control/status wrapper around robot state\n\n enable() - enable all joints\n disable() - disable all joints\n reset() - reset all joints, reset all jrcp faults, disable the robot\n stop() - stop the robot, similar to hitting the e-stop button\n\n :param robot_params: A RobotParams instance (optional)\n :type robot_params: RobotParams\n \"\"\"\n\n param_lock = Lock()\n\n def __init__(self, robot_params = None):\n \"\"\"\n \n \"\"\"\n\n if robot_params:\n self._params = robot_params\n else:\n self._params = RobotParams()\n\n self._ns = self._params.get_base_namespace()\n\n self._enabled = None\n\n state_topic = '{}/custom_franka_state_controller/robot_state'.format(self._ns)\n self._state_sub = rospy.Subscriber(state_topic,\n RobotState,\n self._state_callback\n )\n\n franka_dataflow.wait_for(\n lambda: not self._enabled is None,\n timeout=5.0,\n timeout_msg=(\"Failed to get robot state on %s\" %\n (state_topic,)),\n )\n\n def _state_callback(self, msg):\n self._enabled = (msg.robot_mode != 4) \n\n def is_enabled(self):\n \"\"\"\n Return status of robot\n\n :return: True if enabled, False otherwise\n :rtype: bool\n \"\"\"\n return self._enabled\n \n\n def _toggle_enabled(self, status):\n\n pub = rospy.Publisher('{}/franka_control/error_recovery/goal'.format(self._ns), ErrorRecoveryActionGoal,\n queue_size=10)\n\n if self._enabled == status:\n rospy.loginfo(\"Robot is already %s\"%self.state())\n\n franka_dataflow.wait_for(\n test=lambda: self._enabled == status,\n timeout=5.0,\n timeout_msg=(\"Failed to %sable robot\" %\n ('en' if status else 'dis',)),\n body=lambda: pub.publish(ErrorRecoveryActionGoal()),\n )\n rospy.loginfo(\"Robot %s\", ('Enabled' if status else 'Disabled'))\n\n def state(self):\n \"\"\"\n Returns the last known robot state.\n\n :rtype: str\n :return: \"Enabled\"/\"Disabled\"\n \"\"\"\n return \"%sabled\"%('en' if self._enabled else 'dis',)\n\n def enable(self):\n \"\"\"\n Enable all joints\n \"\"\"\n if not self._enabled:\n rospy.loginfo(\"Robot Stopped: Attempting Reset...\")\n self._toggle_enabled(True)\n\n def disable(self):\n \"\"\"\n Disable all joints\n \"\"\"\n self._toggle_enabled(False)\n " }, { "path": "franka_interface/src/franka_interface/robot_params.py", "content": "# /***************************************************************************\n\n# \n# @package: franka_interface\n# @metapackage: franka_ros_interface\n# @author: Saif Sidhik \n# \n\n# **************************************************************************/\n\n# /***************************************************************************\n# Copyright (c) 2019-2021, Saif Sidhik\n \n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n\n# http://www.apache.org/licenses/LICENSE-2.0\n\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n# **************************************************************************/\n\n\"\"\"\n:info: \n Interface class for collecting and storing necessary parameters from ROS parameter server.\n\n\"\"\"\n\nimport sys\nimport rospy\nimport socket\nfrom franka_core_msgs.msg import JointLimits\n\ndef _log_networking_error():\n print (\"Failed to connect to the ROS parameter server!\\n\"\n \"Please check to make sure your ROS networking is \"\n \"properly configured:\\n\")\n sys.exit()\n\nclass RobotParams(object):\n \"\"\"\n Interface class for essential ROS parameters on the Franka Emika Panda robot.\n \"\"\"\n\n def __init__(self):\n\n if self._robot_in_simulation():\n self._in_sim = True\n self._ns = \"/panda_simulator\"\n rospy.loginfo(\"Robot control running in Simulation Mode!\")\n self._robot_ip=\"sim\"\n else:\n self._ns = \"/franka_ros_interface\" \n self._in_sim = False\n self._robot_ip = self.get_robot_ip()\n\n self._robot_name = self.get_robot_name()\n\n def get_base_namespace(self):\n return self._ns\n\n def _robot_in_simulation(self):\n sim = False\n try:\n sim = rospy.get_param(\"/SIMULATOR_\")\n except KeyError:\n sim = False\n except (socket.error, socket.gaierror):\n _log_networking_error()\n\n return sim\n\n def get_neutral_pose(self):\n \"\"\"\n Get neutral pose joint positions from parameter server \n (/robot_config/neutral_pose)\n\n :return: Joint positions of the robot as defined in parameter server.\n :rtype: [type]\n \"\"\"\n try:\n neutral_pose = rospy.get_param(\"/robot_config/neutral_pose\")\n except KeyError:\n rospy.logerr(\"RobotParam: robot_ip cannot detect neutral joint pos.\"\n \" under param /franka_control/neutral_pose\")\n except (socket.error, socket.gaierror):\n _log_networking_error()\n\n return neutral_pose\n\n\n def get_robot_ip(self):\n robot_ip = None\n try:\n robot_ip = rospy.get_param(\"/franka_control/robot_ip\")\n except KeyError:\n try:\n robot_ip = rospy.get_param(\"/robot_config/robot_ip\")\n except KeyError:\n rospy.logerr(\"RobotParam: robot_ip cannot detect robot ip.\"\n \" under param /robot_ip\")\n except (socket.error, socket.gaierror):\n _log_networking_error()\n\n return robot_ip\n\n def get_joint_names(self):\n \"\"\"\n Return the names of the joints for the robot from ROS parameter\n server (/robot_config/joint_names).\n\n :rtype: [str]\n :return: ordered list of joint names from proximal to distal\n (i.e. shoulder to wrist). joint names for limb\n \"\"\"\n joint_names = list()\n try:\n joint_names = rospy.get_param(\"/robot_config/joint_names\")\n except KeyError:\n rospy.logerr(\"RobotParam: get_joint_names cannot detect joint_names for arm\")\n except (socket.error, socket.gaierror):\n _log_networking_error()\n return joint_names\n\n def get_gripper_joint_names(self):\n \"\"\"\n Return the names of the joints for the gripper from ROS \n parameter server (/gripper_config/joint_names).\n\n :rtype: [str]\n :return: ordered list of joint names\n \"\"\"\n\n joint_names = list()\n try:\n joint_names = rospy.get_param(\"/gripper_config/joint_names\")\n except KeyError:\n rospy.loginfo(\"RobotParam: get_gripper_joint_names cannot detect joint_names for gripper.\")\n return None\n except (socket.error, socket.gaierror):\n _log_networking_error()\n return joint_names\n\n def get_robot_name(self):\n \"\"\"\n Return the name of class of robot from ROS parameter server.\n (/robot_config/arm_id)\n\n :rtype: str\n :return: name of the robot\n \"\"\"\n robot_name = None\n try:\n robot_name = rospy.get_param(\"/robot_config/arm_id\")\n except KeyError:\n rospy.logerr(\"RobotParam: get_robot_name cannot detect robot name\"\n \" under param /robot_config/arm_id\")\n except (socket.error, socket.gaierror):\n _log_networking_error()\n return robot_name\n\n def get_joint_limits(self):\n \"\"\"\n Get joint limits as defined in ROS parameter server \n (/robot_config/joint_config/joint_velocity_limit)\n\n :return: Joint limits for each joints\n :rtype: franka_core_msgs.msg.JointLimits\n \"\"\"\n\n lims = JointLimits()\n lims.joint_names = self.get_joint_names()\n\n try:\n vel_lim = rospy.get_param(\"/robot_config/joint_config/joint_velocity_limit\")\n except KeyError:\n rospy.logerr(\"RobotParam: get_joint_limits cannot detect robot joint velocity limits\"\n \" under param /robot_config/joint_config/joint_velocity_limit\")\n except (socket.error, socket.gaierror):\n _log_networking_error()\n\n try:\n pos_min_lim = rospy.get_param(\"/robot_config/joint_config/joint_position_limit/lower\")\n except KeyError:\n rospy.logerr(\"RobotParam: get_joint_limits cannot detect robot joint position lower limits\"\n \" under param /robot_config/joint_config/joint_position_limit/lower\")\n except (socket.error, socket.gaierror):\n _log_networking_error()\n\n try:\n pos_max_lim = rospy.get_param(\"/robot_config/joint_config/joint_position_limit/upper\")\n except KeyError:\n rospy.logerr(\"RobotParam: get_joint_limits cannot detect robot joint position upper limits\"\n \" under param /robot_config/joint_config/joint_position_limit/upper\")\n except (socket.error, socket.gaierror):\n _log_networking_error()\n\n try:\n eff_lim = rospy.get_param(\"/robot_config/joint_config/joint_effort_limit\")\n except KeyError:\n rospy.logerr(\"RobotParam: get_joint_limits cannot detect robot joint torque limits\"\n \" under param /robot_config/joint_config/joint_effort_limit\")\n except (socket.error, socket.gaierror):\n _log_networking_error()\n\n try:\n acc_lim = rospy.get_param(\"/robot_config/joint_config/joint_acceleration_limit\")\n except KeyError:\n rospy.logerr(\"RobotParam: get_joint_limits cannot detect robot joint acceleration limits\"\n \" under param /robot_config/joint_config/joint_acceleration_limit\")\n except (socket.error, socket.gaierror):\n _log_networking_error()\n\n\n for i in range(len(lims.joint_names)):\n lims.position_upper.append(pos_max_lim[lims.joint_names[i]])\n lims.position_lower.append(pos_min_lim[lims.joint_names[i]])\n lims.velocity.append(vel_lim[lims.joint_names[i]])\n lims.accel.append(acc_lim[lims.joint_names[i]])\n lims.effort.append(eff_lim[lims.joint_names[i]])\n\n return lims\n\n\n\nif __name__ == '__main__':\n \n rp = RobotParams()\n # print rp.__dict__\n print(rp.get_robot_ip())\n print(rp.get_robot_name())\n print(rp.get_joint_names())\n\n\n" }, { "path": "franka_interface/src/motion_controller_interface.cpp", "content": "/***************************************************************************\n* Adapted from arm_controller_interface.cpp (sawyer_simulator package)\n\n*\n* @package: franka_interface\n* @metapackage: franka_ros_interface\n* @author: Saif Sidhik \n*\n\n**************************************************************************/\n\n/***************************************************************************\n* Copyright (c) 2019-2021, Saif Sidhik\n* Copyright (c) 2013-2018, Rethink Robotics Inc.\n*\n* Licensed under the Apache License, Version 2.0 (the \"License\");\n* you may not use this file except in compliance with the License.\n* You may obtain a copy of the License at\n*\n* http://www.apache.org/licenses/LICENSE-2.0\n*\n* Unless required by applicable law or agreed to in writing, software\n* distributed under the License is distributed on an \"AS IS\" BASIS,\n* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n* See the License for the specific language governing permissions and\n* limitations under the License.\n**************************************************************************/\n\n#include \n#include \n\nnamespace franka_interface {\n\nvoid MotionControllerInterface::init(ros::NodeHandle& nh,\n boost::shared_ptr controller_manager) {\n current_mode_ = -1;\n\n if (!nh.getParam(\"/controllers_config/position_controller\", position_controller_name_)) {\n position_controller_name_ = \"position_joint_position_controller\";\n }\n if (!nh.getParam(\"/controllers_config/torque_controller\", torque_controller_name_)) {\n torque_controller_name_ = \"effort_joint_torque_controller\";\n }\n if (!nh.getParam(\"/controllers_config/impedance_controller\", impedance_controller_name_)) {\n impedance_controller_name_ = \"effort_joint_impedance_controller\";\n }\n if (!nh.getParam(\"/controllers_config/velocity_controller\", velocity_controller_name_)) {\n velocity_controller_name_ = \"velocity_joint_velocity_controller\";\n }\n if (!nh.getParam(\"/controllers_config/trajectory_controller\", trajectory_controller_name_)) {\n trajectory_controller_name_ = \"position_joint_trajectory_controller\";\n }\n if (!nh.getParam(\"/controllers_config/default_controller\", default_controller_name_)) {\n default_controller_name_ = \"position_joint_trajectory_controller\";\n }\n\n current_controller_name_ = default_controller_name_;\n\n all_controllers_.clear();\n all_controllers_.push_back(position_controller_name_);\n all_controllers_.push_back(torque_controller_name_);\n all_controllers_.push_back(impedance_controller_name_);\n all_controllers_.push_back(velocity_controller_name_);\n all_controllers_.push_back(trajectory_controller_name_);\n\n bool default_defined = false;\n\n for (size_t i = 0; i < all_controllers_.size(); ++i){\n if (all_controllers_[i] == default_controller_name_){\n default_defined = true;\n break;\n }\n }\n\n controller_name_to_mode_map_[position_controller_name_] = franka_core_msgs::JointCommand::POSITION_MODE;\n controller_name_to_mode_map_[torque_controller_name_] = franka_core_msgs::JointCommand::TORQUE_MODE;\n controller_name_to_mode_map_[impedance_controller_name_] = franka_core_msgs::JointCommand::IMPEDANCE_MODE;\n controller_name_to_mode_map_[velocity_controller_name_] = franka_core_msgs::JointCommand::VELOCITY_MODE;\n controller_name_to_mode_map_[trajectory_controller_name_] = -1;\n\n if (! default_defined){\n ROS_ERROR_STREAM_NAMED(\"MotionControllerInterface\", \"Default controller not present in the provided controllers!\");\n }\n\n controller_manager_ = controller_manager;\n joint_command_sub_ = nh.subscribe(\"/franka_ros_interface/motion_controller/arm/joint_commands\", 1,\n &MotionControllerInterface::jointCommandCallback, this);\n\n // Command Timeout\n joint_command_timeout_sub_ = nh.subscribe(\"/franka_ros_interface/motion_controller/arm/joint_command_timeout\", 1,\n &MotionControllerInterface::jointCommandTimeoutCallback, this);\n double command_timeout_default;\n nh.param(\"/controllers_config/command_timeout\", command_timeout_default, 0.2);\n auto p_cmd_timeout_length = std::make_shared(std::min(1.0,\n std::max(0.0, command_timeout_default)));\n box_timeout_length_.set(p_cmd_timeout_length);\n\n ROS_INFO_STREAM(\"MotionControllerInterface Initialised\");\n\n // Update at 100Hz\n cmd_timeout_timer_ = nh.createTimer(100, &MotionControllerInterface::commandTimeoutCheck, this);\n}\n\nvoid MotionControllerInterface::commandTimeoutCheck(const ros::TimerEvent& e) {\n // lock out other thread(s) which are getting called back via ros.\n std::lock_guard guard(mtx_);\n // Check Command Timeout\n std::shared_ptr p_timeout_length;\n box_timeout_length_.get(p_timeout_length);\n std::shared_ptr p_cmd_msg_time;\n box_cmd_timeout_.get(p_cmd_msg_time);\n bool command_timeout = (p_cmd_msg_time && p_timeout_length &&\n ((ros::Time::now() - *p_cmd_msg_time.get()) > (*p_timeout_length.get())));\n if(command_timeout && (current_controller_name_ != default_controller_name_)) {\n // Timeout violated, force robot back to Default Controller Mode\n\n ROS_WARN_STREAM(\"MotionControllerInterface: Command timeout violated: Switching to Default control mode.\" << default_controller_name_);\n switchToDefaultController();\n }\n}\n\nbool MotionControllerInterface::switchToDefaultController() {\n\n std::vector start_controllers; \n std::vector stop_controllers;\n\n for (size_t i = 0; i < all_controllers_.size(); ++i) {\n\n if (all_controllers_[i] == default_controller_name_)\n start_controllers.push_back(all_controllers_[i]);\n else stop_controllers.push_back(all_controllers_[i]);\n }\n if (!controller_manager_->switchController(start_controllers, stop_controllers,\n controller_manager_msgs::SwitchController::Request::BEST_EFFORT))\n {\n ROS_ERROR_STREAM_NAMED(\"MotionControllerInterface\", \"Failed to switch controllers\");\n return false;\n }\n current_controller_name_ = start_controllers[0];\n current_mode_ = controller_name_to_mode_map_[current_controller_name_];\n ROS_INFO_STREAM(\"MotionControllerInterface: Controller \" << start_controllers[0]\n << \" started; Controllers \" << stop_controllers[0] <<\n \", \" << stop_controllers[1] <<\n \", \" << stop_controllers[2] <<\n \", \" << stop_controllers[3] << \" stopped.\");\n return true;\n}\n\n\nvoid MotionControllerInterface::jointCommandTimeoutCallback(const std_msgs::Float64 msg) {\n ROS_INFO_STREAM(\"MotionControllerInterface: Joint command timeout: \" << msg.data);\n auto p_cmd_timeout_length = std::make_shared(\n std::min(1.0, std::max(0.0, double(msg.data))));\n box_timeout_length_.set(p_cmd_timeout_length);\n}\n\nbool MotionControllerInterface::switchControllers(int control_mode) {\n std::vector start_controllers;\n std::vector stop_controllers;\n\n if(current_mode_ != control_mode)\n {\n switch (control_mode)\n {\n case franka_core_msgs::JointCommand::POSITION_MODE:\n start_controllers.push_back(position_controller_name_);\n stop_controllers.push_back(impedance_controller_name_);\n stop_controllers.push_back(torque_controller_name_);\n stop_controllers.push_back(velocity_controller_name_);\n stop_controllers.push_back(trajectory_controller_name_);\n break;\n case franka_core_msgs::JointCommand::IMPEDANCE_MODE:\n start_controllers.push_back(impedance_controller_name_);\n stop_controllers.push_back(position_controller_name_);\n stop_controllers.push_back(torque_controller_name_);\n stop_controllers.push_back(velocity_controller_name_);\n stop_controllers.push_back(trajectory_controller_name_);\n break;\n case franka_core_msgs::JointCommand::TORQUE_MODE:\n start_controllers.push_back(torque_controller_name_);\n stop_controllers.push_back(position_controller_name_);\n stop_controllers.push_back(impedance_controller_name_);\n stop_controllers.push_back(velocity_controller_name_);\n stop_controllers.push_back(trajectory_controller_name_);\n break;\n case franka_core_msgs::JointCommand::VELOCITY_MODE:\n start_controllers.push_back(velocity_controller_name_);\n stop_controllers.push_back(position_controller_name_);\n stop_controllers.push_back(impedance_controller_name_);\n stop_controllers.push_back(torque_controller_name_);\n stop_controllers.push_back(trajectory_controller_name_);\n break; \n default:\n ROS_ERROR_STREAM_NAMED(\"MotionControllerInterface\", \"Unknown JointCommand mode \"\n << control_mode << \". Ignoring command.\");\n return false;\n }\n if (!controller_manager_->switchController(start_controllers, stop_controllers,\n controller_manager_msgs::SwitchController::Request::BEST_EFFORT))\n {\n ROS_ERROR_STREAM_NAMED(\"MotionControllerInterface\", \"Failed to switch controllers\");\n return false;\n }\n current_mode_ = control_mode;\n current_controller_name_ = start_controllers[0];\n ROS_INFO_STREAM(\"MotionControllerInterface: Controller \" << start_controllers[0]\n << \" started; Controllers \" << stop_controllers[0] <<\n \", \" << stop_controllers[1] <<\n \", \" << stop_controllers[2] <<\n \", \" << stop_controllers[3] << \" stopped.\");\n }\n return true;\n}\n\nvoid MotionControllerInterface::jointCommandCallback(const franka_core_msgs::JointCommandConstPtr& msg) {\n // lock out other thread(s) which are getting called back via ros.\n std::lock_guard guard(mtx_);\n if(switchControllers(msg->mode)) {\n auto p_cmd_msg_time = std::make_shared(ros::Time::now());\n box_cmd_timeout_.set(p_cmd_msg_time);\n }\n}\n\n}\n" }, { "path": "franka_interface/src/robot_state_controller.cpp", "content": "/***************************************************************************\n* Adapted from robot_state_publisher.cpp (franka_ros package)\n\n*\n* @package: panda_gazebo\n* @metapackage: panda_simulator\n* @author: Saif Sidhik \n*\n\n**************************************************************************/\n\n/***************************************************************************\n* Copyright (c) 2019-2021, Saif Sidhik.\n* Copyright (c) 2017 Franka Emika GmbH\n*\n* Licensed under the Apache License, Version 2.0 (the \"License\");\n* you may not use this file except in compliance with the License.\n* You may obtain a copy of the License at\n*\n* http://www.apache.org/licenses/LICENSE-2.0\n*\n* Unless required by applicable law or agreed to in writing, software\n* distributed under the License is distributed on an \"AS IS\" BASIS,\n* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n* See the License for the specific language governing permissions and\n* limitations under the License.\n**************************************************************************/\n\n#include \n\n#include \n#include \n#include \n#include \n\n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n\nnamespace {\n\ntf::Transform convertArrayToTf(const std::array& transform) {\n tf::Matrix3x3 rotation(transform[0], transform[4], transform[8], transform[1], transform[5],\n transform[9], transform[2], transform[6], transform[10]);\n tf::Vector3 translation(transform[12], transform[13], transform[14]);\n return tf::Transform(rotation, translation);\n}\n\nfranka_msgs::Errors errorsToMessage(const franka::Errors& error) {\n franka_msgs::Errors message;\n message.joint_position_limits_violation =\n static_cast(\n error.joint_position_limits_violation);\n message.cartesian_position_limits_violation =\n static_cast(\n error.cartesian_position_limits_violation);\n message.self_collision_avoidance_violation =\n static_cast(\n error.self_collision_avoidance_violation);\n message.joint_velocity_violation =\n static_cast(error.joint_velocity_violation);\n message.cartesian_velocity_violation =\n static_cast(\n error.cartesian_velocity_violation);\n message.force_control_safety_violation =\n static_cast(\n error.force_control_safety_violation);\n message.joint_reflex = static_cast(error.joint_reflex);\n message.cartesian_reflex =\n static_cast(error.cartesian_reflex);\n message.max_goal_pose_deviation_violation =\n static_cast(\n error.max_goal_pose_deviation_violation);\n message.max_path_pose_deviation_violation =\n static_cast(\n error.max_path_pose_deviation_violation);\n message.cartesian_velocity_profile_safety_violation =\n static_cast(\n error.cartesian_velocity_profile_safety_violation);\n message.joint_position_motion_generator_start_pose_invalid =\n static_cast(\n error.joint_position_motion_generator_start_pose_invalid);\n message.joint_motion_generator_position_limits_violation =\n static_cast(\n error.joint_motion_generator_position_limits_violation);\n message.joint_motion_generator_velocity_limits_violation =\n static_cast(\n error.joint_motion_generator_velocity_limits_violation);\n message.joint_motion_generator_velocity_discontinuity =\n static_cast(\n error.joint_motion_generator_velocity_discontinuity);\n message.joint_motion_generator_acceleration_discontinuity =\n static_cast(\n error.joint_motion_generator_acceleration_discontinuity);\n message.cartesian_position_motion_generator_start_pose_invalid =\n static_cast(\n error.cartesian_position_motion_generator_start_pose_invalid);\n message.cartesian_motion_generator_elbow_limit_violation =\n static_cast(\n error.cartesian_motion_generator_elbow_limit_violation);\n message.cartesian_motion_generator_velocity_limits_violation =\n static_cast(\n error.cartesian_motion_generator_velocity_limits_violation);\n message.cartesian_motion_generator_velocity_discontinuity =\n static_cast(\n error.cartesian_motion_generator_velocity_discontinuity);\n message.cartesian_motion_generator_acceleration_discontinuity =\n static_cast(\n error.cartesian_motion_generator_acceleration_discontinuity);\n message.cartesian_motion_generator_elbow_sign_inconsistent =\n static_cast(\n error.cartesian_motion_generator_elbow_sign_inconsistent);\n message.cartesian_motion_generator_start_elbow_invalid =\n static_cast(\n error.cartesian_motion_generator_start_elbow_invalid);\n message.cartesian_motion_generator_joint_position_limits_violation =\n static_cast(\n error.cartesian_motion_generator_joint_position_limits_violation);\n message.cartesian_motion_generator_joint_velocity_limits_violation =\n static_cast(\n error.cartesian_motion_generator_joint_velocity_limits_violation);\n message.cartesian_motion_generator_joint_velocity_discontinuity =\n static_cast(\n error.cartesian_motion_generator_joint_velocity_discontinuity);\n message.cartesian_motion_generator_joint_acceleration_discontinuity =\n static_cast(\n error.cartesian_motion_generator_joint_acceleration_discontinuity);\n message.cartesian_position_motion_generator_invalid_frame =\n static_cast(\n error.cartesian_position_motion_generator_invalid_frame);\n message.force_controller_desired_force_tolerance_violation =\n static_cast(\n error.force_controller_desired_force_tolerance_violation);\n message.controller_torque_discontinuity =\n static_cast(\n error.controller_torque_discontinuity);\n message.start_elbow_sign_inconsistent =\n static_cast(\n error.start_elbow_sign_inconsistent);\n message.communication_constraints_violation =\n static_cast(\n error.communication_constraints_violation);\n message.power_limit_violation =\n static_cast(error.power_limit_violation);\n message.joint_p2p_insufficient_torque_for_planning =\n static_cast(\n error.joint_p2p_insufficient_torque_for_planning);\n message.tau_j_range_violation =\n static_cast(error.tau_j_range_violation);\n message.instability_detected =\n static_cast(error.instability_detected);\n return message;\n}\n\n} // anonymous namespace\n\nnamespace franka_interface {\n\nbool CustomFrankaStateController::init(hardware_interface::RobotHW* robot_hardware,\n ros::NodeHandle& root_node_handle,\n ros::NodeHandle& controller_node_handle) {\n franka_state_interface_ = robot_hardware->get();\n if (franka_state_interface_ == nullptr) {\n ROS_ERROR(\"CustomFrankaStateController: Could not get Franka state interface from hardware\");\n return false;\n }\n if (!controller_node_handle.getParam(\"arm_id\", arm_id_)) {\n ROS_ERROR(\"CustomFrankaStateController: Could not get parameter arm_id\");\n return false;\n }\n double publish_rate(500.0);\n if (!controller_node_handle.getParam(\"publish_rate\", publish_rate)) {\n ROS_INFO_STREAM(\"CustomFrankaStateController: Did not find publish_rate. Using default \"\n << publish_rate << \" [Hz].\");\n }\n trigger_publish_ = franka_hw::TriggerRate(publish_rate);\n\n if (!controller_node_handle.getParam(\"joint_names\", joint_names_) ||\n joint_names_.size() != robot_state_.q.size()) {\n ROS_ERROR(\n \"CustomFrankaStateController: Invalid or no joint_names parameters provided, aborting \"\n \"controller init!\");\n return false;\n }\n\n try {\n franka_state_handle_ = std::make_unique(\n franka_state_interface_->getHandle(arm_id_ + \"_robot\"));\n } catch (const hardware_interface::HardwareInterfaceException& ex) {\n ROS_ERROR_STREAM(\"CustomFrankaStateController: Exception getting franka state handle: \" << ex.what());\n return false;\n }\n\n auto* model_interface = robot_hardware->get();\n if (model_interface == nullptr) {\n ROS_ERROR_STREAM(\n \"CustomFrankaStateController: Error getting model interface from hardware\");\n return false;\n }\n try {\n model_handle_ = std::make_unique(\n model_interface->getHandle(arm_id_ + \"_model\"));\n } catch (hardware_interface::HardwareInterfaceException& ex) {\n ROS_ERROR_STREAM(\n \"CustomFrankaStateController: Exception getting model handle from interface: \"\n << ex.what());\n return false;\n }\n\n publisher_transforms_.init(root_node_handle, \"/tf\", 1);\n publisher_franka_state_.init(controller_node_handle, \"robot_state\", 1);\n publisher_joint_states_.init(controller_node_handle, \"joint_states\", 1);\n publisher_joint_states_desired_.init(controller_node_handle, \"joint_states_desired\", 1);\n publisher_tip_state_.init(controller_node_handle, \"tip_state\", 1);\n\n {\n std::lock_guard > lock(\n publisher_joint_states_);\n publisher_joint_states_.msg_.name.resize(joint_names_.size());\n publisher_joint_states_.msg_.position.resize(robot_state_.q.size());\n publisher_joint_states_.msg_.velocity.resize(robot_state_.dq.size());\n publisher_joint_states_.msg_.effort.resize(robot_state_.tau_J.size());\n }\n {\n std::lock_guard > lock(\n publisher_joint_states_desired_);\n publisher_joint_states_desired_.msg_.name.resize(joint_names_.size());\n publisher_joint_states_desired_.msg_.position.resize(robot_state_.q_d.size());\n publisher_joint_states_desired_.msg_.velocity.resize(robot_state_.dq_d.size());\n publisher_joint_states_desired_.msg_.effort.resize(robot_state_.tau_J_d.size());\n }\n {\n std::lock_guard > lock(\n publisher_transforms_);\n publisher_transforms_.msg_.transforms.resize(3);\n tf::Quaternion quaternion(0.0, 0.0, 0.0, 1.0);\n tf::Vector3 translation(0.0, 0.0, 0.0);\n tf::Transform transform(quaternion, translation);\n tf::StampedTransform stamped_transform(transform, ros::Time::now(), arm_id_ + \"_link8\",\n arm_id_ + \"_NE\");\n geometry_msgs::TransformStamped transform_message;\n transformStampedTFToMsg(stamped_transform, transform_message);\n publisher_transforms_.msg_.transforms[0] = transform_message;\n\n translation = tf::Vector3(0.0, 0.0, 0.0);\n transform = tf::Transform(quaternion, translation);\n stamped_transform =\n tf::StampedTransform(transform, ros::Time::now(), arm_id_ + \"_NE\", arm_id_ + \"_EE\");\n transformStampedTFToMsg(stamped_transform, transform_message);\n publisher_transforms_.msg_.transforms[1] = transform_message;\n\n translation = tf::Vector3(0.0, 0.0, 0.0);\n transform = tf::Transform(quaternion, translation);\n stamped_transform =\n tf::StampedTransform(transform, ros::Time::now(), arm_id_ + \"_EE\", arm_id_ + \"_K\");\n transformStampedTFToMsg(stamped_transform, transform_message);\n publisher_transforms_.msg_.transforms[2] = transform_message;\n }\n {\n std::lock_guard > lock(\n publisher_tip_state_);\n publisher_tip_state_.msg_.O_F_ext_hat_K.header.frame_id = arm_id_ + \"_link0\";\n publisher_tip_state_.msg_.O_F_ext_hat_K.wrench.force.x = 0.0;\n publisher_tip_state_.msg_.O_F_ext_hat_K.wrench.force.y = 0.0;\n publisher_tip_state_.msg_.O_F_ext_hat_K.wrench.force.z = 0.0;\n publisher_tip_state_.msg_.O_F_ext_hat_K.wrench.torque.x = 0.0;\n publisher_tip_state_.msg_.O_F_ext_hat_K.wrench.torque.y = 0.0;\n publisher_tip_state_.msg_.O_F_ext_hat_K.wrench.torque.z = 0.0;\n\n publisher_tip_state_.msg_.K_F_ext_hat_K.header.frame_id = arm_id_ + \"_K\";\n publisher_tip_state_.msg_.K_F_ext_hat_K.wrench.force.x = 0.0;\n publisher_tip_state_.msg_.K_F_ext_hat_K.wrench.force.y = 0.0;\n publisher_tip_state_.msg_.K_F_ext_hat_K.wrench.force.z = 0.0;\n publisher_tip_state_.msg_.K_F_ext_hat_K.wrench.torque.x = 0.0;\n publisher_tip_state_.msg_.K_F_ext_hat_K.wrench.torque.y = 0.0;\n publisher_tip_state_.msg_.K_F_ext_hat_K.wrench.torque.z = 0.0;\n }\n return true;\n}\n\nvoid CustomFrankaStateController::update(const ros::Time& time, const ros::Duration& /* period */) {\n if (trigger_publish_()) {\n robot_state_ = franka_state_handle_->getRobotState();\n publishFrankaState(time);\n publishTransforms(time);\n publishEndPointState(time);\n publishJointStates(time);\n sequence_number_++;\n }\n}\n\nvoid CustomFrankaStateController::publishFrankaState(const ros::Time& time) {\n\n std::array coriolis = model_handle_->getCoriolis();\n std::array gravity = model_handle_->getGravity();\n\n std::array mass_matrix = model_handle_->getMass();\n std::array O_Jac_EE = model_handle_->getZeroJacobian(franka::Frame::kEndEffector);\n Eigen::Map> jacobian(O_Jac_EE.data());\n Eigen::Map> dq(robot_state_.dq.data());\n\n// jacobian * dq\n Eigen::Matrix ee_vel = jacobian * dq;\n\n if (publisher_franka_state_.trylock()) {\n static_assert(\n sizeof(robot_state_.cartesian_collision) == sizeof(robot_state_.cartesian_contact),\n \"Robot state Cartesian members do not have same size\");\n static_assert(\n sizeof(robot_state_.cartesian_collision) == sizeof(robot_state_.O_dP_EE_c),\n \"Robot state Cartesian members do not have same size\");\n static_assert(\n sizeof(robot_state_.cartesian_collision) == sizeof(robot_state_.O_dP_EE_d),\n \"Robot state Cartesian members do not have same size\");\n static_assert(\n sizeof(robot_state_.cartesian_collision) == sizeof(robot_state_.O_ddP_EE_c),\n \"Robot state Cartesian members do not have same size\");\n for (size_t i = 0; i < robot_state_.cartesian_collision.size(); i++) {\n publisher_franka_state_.msg_.cartesian_collision[i] = robot_state_.cartesian_collision[i];\n publisher_franka_state_.msg_.cartesian_contact[i] = robot_state_.cartesian_contact[i];\n publisher_franka_state_.msg_.O_dP_EE[i] = ee_vel(i,0);\n }\n\n static_assert(sizeof(robot_state_.q) == sizeof(robot_state_.q_d),\n \"Robot state joint members do not have same size\");\n static_assert(sizeof(robot_state_.q) == sizeof(robot_state_.dq_d),\n \"Robot state joint members do not have same size\");\n static_assert(sizeof(robot_state_.q) == sizeof(robot_state_.dtau_J),\n \"Robot state joint members do not have same size\");\n static_assert(sizeof(robot_state_.q) == sizeof(robot_state_.tau_J_d),\n \"Robot state joint members do not have same size\");\n static_assert(sizeof(robot_state_.q) == sizeof(robot_state_.joint_collision),\n \"Robot state joint members do not have same size\");\n static_assert(sizeof(robot_state_.q) == sizeof(robot_state_.joint_contact),\n \"Robot state joint members do not have same size\");\n static_assert(sizeof(robot_state_.q) == sizeof(robot_state_.tau_ext_hat_filtered),\n \"Robot state joint members do not have same size\");\n for (size_t i = 0; i < robot_state_.q.size(); i++) {\n publisher_franka_state_.msg_.q_d[i] = robot_state_.q_d[i];\n publisher_franka_state_.msg_.dq_d[i] = robot_state_.dq_d[i];\n publisher_franka_state_.msg_.dtau_J[i] = robot_state_.dtau_J[i];\n publisher_franka_state_.msg_.tau_J_d[i] = robot_state_.tau_J_d[i];\n publisher_franka_state_.msg_.joint_collision[i] = robot_state_.joint_collision[i];\n publisher_franka_state_.msg_.joint_contact[i] = robot_state_.joint_contact[i];\n publisher_franka_state_.msg_.tau_ext_hat_filtered[i] = robot_state_.tau_ext_hat_filtered[i];\n publisher_franka_state_.msg_.gravity[i] = gravity[i];\n publisher_franka_state_.msg_.coriolis[i] = coriolis[i];\n }\n\n static_assert(sizeof(robot_state_.O_T_EE) == sizeof(robot_state_.F_T_EE),\n \"Robot state transforms do not have same size\");\n static_assert(sizeof(robot_state_.O_T_EE) == sizeof(robot_state_.EE_T_K),\n \"Robot state transforms do not have same size\");\n static_assert(sizeof(robot_state_.O_T_EE) == sizeof(robot_state_.O_T_EE_d),\n \"Robot state transforms do not have same size\");\n static_assert(sizeof(robot_state_.NE_T_EE) == sizeof(robot_state_.O_T_EE),\n \"Robot state transforms do not have same size\");\n\n for (size_t i = 0; i < robot_state_.O_T_EE.size(); i++)\n {\n publisher_franka_state_.msg_.F_T_EE[i] = robot_state_.F_T_EE[i];\n publisher_franka_state_.msg_.EE_T_K[i] = robot_state_.EE_T_K[i];\n publisher_franka_state_.msg_.O_T_EE_d[i] = robot_state_.O_T_EE_d[i];\n publisher_franka_state_.msg_.F_T_NE[i] = robot_state_.F_T_NE[i];\n publisher_franka_state_.msg_.NE_T_EE[i] = robot_state_.NE_T_EE[i];\n }\n publisher_franka_state_.msg_.m_ee = robot_state_.m_ee;\n publisher_franka_state_.msg_.m_load = robot_state_.m_load;\n publisher_franka_state_.msg_.m_total = robot_state_.m_total;\n\n for (size_t i = 0; i < robot_state_.I_load.size(); i++) {\n publisher_franka_state_.msg_.I_ee[i] = robot_state_.I_ee[i];\n publisher_franka_state_.msg_.I_load[i] = robot_state_.I_load[i];\n publisher_franka_state_.msg_.I_total[i] = robot_state_.I_total[i];\n }\n\n for (size_t i = 0; i < robot_state_.F_x_Cload.size(); i++) {\n publisher_franka_state_.msg_.F_x_Cee[i] = robot_state_.F_x_Cee[i];\n publisher_franka_state_.msg_.F_x_Cload[i] = robot_state_.F_x_Cload[i];\n publisher_franka_state_.msg_.F_x_Ctotal[i] = robot_state_.F_x_Ctotal[i];\n }\n\n for (size_t i = 0; i < mass_matrix.size(); i++) {\n publisher_franka_state_.msg_.mass_matrix[i] = mass_matrix[i];\n }\n\n for (size_t i = 0; i < O_Jac_EE.size(); i++) {\n publisher_franka_state_.msg_.O_Jac_EE[i] = O_Jac_EE[i];\n }\n\n publisher_franka_state_.msg_.time = robot_state_.time.toSec();\n publisher_franka_state_.msg_.current_errors = errorsToMessage(robot_state_.current_errors);\n publisher_franka_state_.msg_.last_motion_errors =\n errorsToMessage(robot_state_.last_motion_errors);\n\n switch (robot_state_.robot_mode) {\n case franka::RobotMode::kOther:\n publisher_franka_state_.msg_.robot_mode = franka_core_msgs::RobotState::ROBOT_MODE_OTHER;\n break;\n\n case franka::RobotMode::kIdle:\n publisher_franka_state_.msg_.robot_mode = franka_core_msgs::RobotState::ROBOT_MODE_IDLE;\n break;\n\n case franka::RobotMode::kMove:\n publisher_franka_state_.msg_.robot_mode = franka_core_msgs::RobotState::ROBOT_MODE_MOVE;\n break;\n\n case franka::RobotMode::kGuiding:\n publisher_franka_state_.msg_.robot_mode = franka_core_msgs::RobotState::ROBOT_MODE_GUIDING;\n break;\n\n case franka::RobotMode::kReflex:\n publisher_franka_state_.msg_.robot_mode = franka_core_msgs::RobotState::ROBOT_MODE_REFLEX;\n break;\n\n case franka::RobotMode::kUserStopped:\n publisher_franka_state_.msg_.robot_mode =\n franka_core_msgs::RobotState::ROBOT_MODE_USER_STOPPED;\n break;\n\n case franka::RobotMode::kAutomaticErrorRecovery:\n publisher_franka_state_.msg_.robot_mode =\n franka_core_msgs::RobotState::ROBOT_MODE_AUTOMATIC_ERROR_RECOVERY;\n break;\n }\n\n publisher_franka_state_.msg_.header.seq = sequence_number_;\n publisher_franka_state_.msg_.header.stamp = time;\n publisher_franka_state_.unlockAndPublish();\n }\n}\n\nvoid CustomFrankaStateController::publishJointStates(const ros::Time& time) {\n if (publisher_joint_states_.trylock()) {\n static_assert(sizeof(robot_state_.q) == sizeof(robot_state_.dq),\n \"Robot state joint members do not have same size\");\n static_assert(sizeof(robot_state_.q) == sizeof(robot_state_.tau_J),\n \"Robot state joint members do not have same size\");\n for (size_t i = 0; i < robot_state_.q.size(); i++) {\n publisher_joint_states_.msg_.name[i] = joint_names_[i];\n publisher_joint_states_.msg_.position[i] = robot_state_.q[i];\n publisher_joint_states_.msg_.velocity[i] = robot_state_.dq[i];\n publisher_joint_states_.msg_.effort[i] = robot_state_.tau_J[i];\n }\n publisher_joint_states_.msg_.header.stamp = time;\n publisher_joint_states_.msg_.header.seq = sequence_number_;\n publisher_joint_states_.unlockAndPublish();\n }\n if (publisher_joint_states_desired_.trylock()) {\n static_assert(sizeof(robot_state_.q_d) == sizeof(robot_state_.dq_d),\n \"Robot state joint members do not have same size\");\n static_assert(sizeof(robot_state_.q_d) == sizeof(robot_state_.tau_J_d),\n \"Robot state joint members do not have same size\");\n for (size_t i = 0; i < robot_state_.q_d.size(); i++) {\n publisher_joint_states_desired_.msg_.name[i] = joint_names_[i];\n publisher_joint_states_desired_.msg_.position[i] = robot_state_.q_d[i];\n publisher_joint_states_desired_.msg_.velocity[i] = robot_state_.dq_d[i];\n publisher_joint_states_desired_.msg_.effort[i] = robot_state_.tau_J_d[i];\n }\n publisher_joint_states_desired_.msg_.header.stamp = time;\n publisher_joint_states_desired_.msg_.header.seq = sequence_number_;\n publisher_joint_states_desired_.unlockAndPublish();\n }\n}\n\nvoid CustomFrankaStateController::publishTransforms(const ros::Time& time) {\n if (publisher_transforms_.trylock()) {\n tf::StampedTransform stamped_transform(convertArrayToTf(robot_state_.F_T_NE), time,\n arm_id_ + \"_link8\", arm_id_ + \"_NE\");\n geometry_msgs::TransformStamped transform_message;\n transformStampedTFToMsg(stamped_transform, transform_message);\n publisher_transforms_.msg_.transforms[0] = transform_message;\n\n stamped_transform = tf::StampedTransform(convertArrayToTf(robot_state_.NE_T_EE), time,\n arm_id_ + \"_NE\", arm_id_ + \"_EE\");\n transformStampedTFToMsg(stamped_transform, transform_message);\n publisher_transforms_.msg_.transforms[1] = transform_message;\n\n stamped_transform = tf::StampedTransform(convertArrayToTf(robot_state_.EE_T_K), time,\n arm_id_ + \"_EE\", arm_id_ + \"_K\");\n transformStampedTFToMsg(stamped_transform, transform_message);\n publisher_transforms_.msg_.transforms[2] = transform_message;\n \n publisher_transforms_.unlockAndPublish();\n }\n}\n\nvoid CustomFrankaStateController::publishEndPointState(const ros::Time& time) {\n if (publisher_tip_state_.trylock()) {\n for (size_t i = 0; i < robot_state_.O_T_EE.size(); i++) {\n publisher_tip_state_.msg_.O_T_EE[i] = robot_state_.O_T_EE[i];\n }\n// for (size_t i = 0; i < robot_state_.O_dP_EE_c.size(); i++) {\n// publisher_tip_state_.msg_.O_dP_EE_c[i] = robot_state_.O_dP_EE_c[i];\n// publisher_tip_state_.msg_.O_dP_EE_d[i] = robot_state_.O_dP_EE_d[i];\n// publisher_tip_state_.msg_.O_ddP_EE_c[i] = robot_state_.O_ddP_EE_c[i];\n// }\n publisher_tip_state_.msg_.O_F_ext_hat_K.header.frame_id = arm_id_ + \"_link0\";\n publisher_tip_state_.msg_.O_F_ext_hat_K.header.stamp = time;\n publisher_tip_state_.msg_.O_F_ext_hat_K.wrench.force.x = robot_state_.O_F_ext_hat_K[0];\n publisher_tip_state_.msg_.O_F_ext_hat_K.wrench.force.y = robot_state_.O_F_ext_hat_K[1];\n publisher_tip_state_.msg_.O_F_ext_hat_K.wrench.force.z = robot_state_.O_F_ext_hat_K[2];\n publisher_tip_state_.msg_.O_F_ext_hat_K.wrench.torque.x = robot_state_.O_F_ext_hat_K[3];\n publisher_tip_state_.msg_.O_F_ext_hat_K.wrench.torque.y = robot_state_.O_F_ext_hat_K[4];\n publisher_tip_state_.msg_.O_F_ext_hat_K.wrench.torque.z = robot_state_.O_F_ext_hat_K[5];\n\n publisher_tip_state_.msg_.K_F_ext_hat_K.header.frame_id = arm_id_ + \"_K\";\n publisher_tip_state_.msg_.K_F_ext_hat_K.header.stamp = time;\n publisher_tip_state_.msg_.K_F_ext_hat_K.wrench.force.x = robot_state_.K_F_ext_hat_K[0];\n publisher_tip_state_.msg_.K_F_ext_hat_K.wrench.force.y = robot_state_.K_F_ext_hat_K[1];\n publisher_tip_state_.msg_.K_F_ext_hat_K.wrench.force.z = robot_state_.K_F_ext_hat_K[2];\n publisher_tip_state_.msg_.K_F_ext_hat_K.wrench.torque.x = robot_state_.K_F_ext_hat_K[3];\n publisher_tip_state_.msg_.K_F_ext_hat_K.wrench.torque.y = robot_state_.K_F_ext_hat_K[4];\n publisher_tip_state_.msg_.K_F_ext_hat_K.wrench.torque.z = robot_state_.K_F_ext_hat_K[5];\n\n publisher_tip_state_.msg_.header.frame_id = arm_id_ + \"_link0\";\n publisher_tip_state_.msg_.header.seq = sequence_number_;\n publisher_tip_state_.msg_.header.stamp = time;\n\n publisher_tip_state_.unlockAndPublish();\n }\n}\n\n} // namespace franka_interface\n\nPLUGINLIB_EXPORT_CLASS(franka_interface::CustomFrankaStateController, controller_interface::ControllerBase)\n\n\n" }, { "path": "franka_interface/state_controller_plugin.xml", "content": "\n \n A controller that publishes the complete robot state\n \n" }, { "path": "franka_interface/tests/demo_interface.py", "content": "import rospy\nimport numpy as np\nfrom franka_interface import ArmInterface\n\n\"\"\"\n:info:\n Demo showing ways to use API. Also shows how to move the robot using low-level controllers.\n\n WARNING: The robot will move slightly (small arc swinging motion side-to-side) till code is killed.\n\"\"\"\n\nif __name__ == '__main__':\n rospy.init_node(\"test_robot\")\n r = ArmInterface() # create arm interface instance (see https://justagist.github.io/franka_ros_interface/DOC.html#arminterface for all available methods for ArmInterface() object)\n cm = r.get_controller_manager() # get controller manager instance associated with the robot (not required in most cases)\n mvt = r.get_movegroup_interface() # get the moveit interface for planning and executing trajectories using moveit planners (see https://justagist.github.io/franka_ros_interface/DOC.html#franka_moveit.PandaMoveGroupInterface for documentation)\n\n elapsed_time_ = rospy.Duration(0.0)\n period = rospy.Duration(0.005)\n\n r.move_to_neutral() # move robot to neutral pose\n\n initial_pose = r.joint_angles() # get current joint angles of the robot\n\n jac = r.zero_jacobian() # get end-effector jacobian\n\n count = 0\n rate = rospy.Rate(1000)\n\n rospy.loginfo(\"Commanding...\\n\")\n joint_names = r.joint_names()\n vals = r.joint_angles()\n while not rospy.is_shutdown():\n\n elapsed_time_ += period\n\n delta = 3.14 / 16.0 * (1 - np.cos(3.14 / 5.0 * elapsed_time_.to_sec())) * 0.2\n\n for j in joint_names:\n if j == joint_names[4]:\n vals[j] = initial_pose[j] - delta\n else:\n vals[j] = initial_pose[j] + delta\n\n # r.set_joint_positions(vals) # for position control. Slightly jerky motion.\n r.set_joint_positions_velocities([vals[j] for j in joint_names], [0.0]*7) # for impedance control\n rate.sleep()" }, { "path": "franka_interface/tests/demo_joint_positions_keyboard.py", "content": "#!/usr/bin/env python\n\n# /***************************************************************************\n\n# \n# @package: franka_tools\n# @metapackage: franka_ros_interface\n# @author: Saif Sidhik \n# \n\n# **************************************************************************/\n\n# /***************************************************************************\n# Copyright (c) 2019-2021, Saif Sidhik\n# Copyright (c) 2013-2018, Rethink Robotics Inc.\n \n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n\n# http://www.apache.org/licenses/LICENSE-2.0\n\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n# **************************************************************************/\n\nimport rospy\nfrom future.utils import viewitems # for python2&3 efficient compatibility\n\nfrom franka_interface import ArmInterface, GripperInterface\nfrom franka_dataflow.getch import getch\n\n\"\"\"\nFranka ROS Interface Joint Position Example: Keyboard Control\n\nUse your dev machine's keyboard to control joint positions.\n\nEach key corresponds to increasing or decreasing the angle\nof a joint on the robot arm. The increasing and descreasing\nare represented by number key and letter key next to the number.\n\n:info:\n Demo showing low-level position control using Franka ROS Interface.\n Run the demo and press '?' on keyboard for command instructions.\n\n Disclaimer: This code is only for demonstration purpose, and does not\n show the ideal way to use the interface code.\n\n Warning: The robot will move according to the keyboard press. Also,\n note that the motion will be slightly jerky (small noise from the\n robot joints). This is because of the non-smooth commands sent to\n the robot.\n\"\"\"\n\ndef map_keyboard():\n \"\"\"\n Map keyboard keys to robot joint motion. Keybindings can be \n found when running the script.\n \"\"\"\n\n limb = ArmInterface()\n\n gripper = GripperInterface(ns = limb.get_robot_params().get_base_namespace())\n\n has_gripper = gripper.exists\n\n joints = limb.joint_names()\n\n def set_j(limb, joint_name, delta):\n joint_command = limb.joint_angles()\n joint_command[joint_name] += delta\n limb.set_joint_positions(joint_command)\n # limb.set_joint_positions_velocities([joint_command[j] for j in joints], [0.00001]*7) # impedance control when using this example might fail because commands are sent too quickly for the robot to respond\n\n def set_g(action):\n if has_gripper:\n if action == \"close\":\n gripper.close()\n elif action == \"open\":\n gripper.open()\n elif action == \"calibrate\":\n gripper.calibrate()\n\n bindings = {\n '1': (set_j, [limb, joints[0], 0.01], joints[0]+\" increase\"),\n 'q': (set_j, [limb, joints[0], -0.01], joints[0]+\" decrease\"),\n '2': (set_j, [limb, joints[1], 0.01], joints[1]+\" increase\"),\n 'w': (set_j, [limb, joints[1], -0.01], joints[1]+\" decrease\"),\n '3': (set_j, [limb, joints[2], 0.01], joints[2]+\" increase\"),\n 'e': (set_j, [limb, joints[2], -0.01], joints[2]+\" decrease\"),\n '4': (set_j, [limb, joints[3], 0.01], joints[3]+\" increase\"),\n 'r': (set_j, [limb, joints[3], -0.01], joints[3]+\" decrease\"),\n '5': (set_j, [limb, joints[4], 0.01], joints[4]+\" increase\"),\n 't': (set_j, [limb, joints[4], -0.01], joints[4]+\" decrease\"),\n '6': (set_j, [limb, joints[5], 0.01], joints[5]+\" increase\"),\n 'y': (set_j, [limb, joints[5], -0.01], joints[5]+\" decrease\"),\n '7': (set_j, [limb, joints[6], 0.01], joints[6]+\" increase\"),\n 'u': (set_j, [limb, joints[6], -0.01], joints[6]+\" decrease\")\n }\n if has_gripper:\n bindings.update({\n '8': (set_g, \"close\", \"close gripper\"),\n 'i': (set_g, \"open\", \"open gripper\"),\n '9': (set_g, \"calibrate\", \"calibrate gripper\")\n })\n done = False\n rospy.logwarn(\"Controlling joints. Press ? for help, Esc to quit.\\n\\nWARNING: The motion will be slightly jerky!!\\n\")\n while not done and not rospy.is_shutdown():\n c = getch()\n if c:\n #catch Esc or ctrl-c\n if c in ['\\x1b', '\\x03']:\n done = True\n rospy.signal_shutdown(\"Example finished.\")\n elif c in bindings:\n cmd = bindings[c]\n if c == '8' or c == 'i' or c == '9':\n cmd[0](cmd[1])\n print(\"command: %s\" % (cmd[2],))\n else:\n #expand binding to something like \"set_j(right, 'j0', 0.1)\"\n cmd[0](*cmd[1])\n print(\"command: %s\" % (cmd[2],))\n else:\n print(\"key bindings: \")\n print(\" Esc: Quit\")\n print(\" ?: Help\")\n for key, val in sorted(viewitems(bindings),\n key=lambda x: x[1][2]):\n print(\" %s: %s\" % (key, val[2]))\n # rospy.sleep(0.005)\n\ndef main():\n\n print(\"Initializing node... \")\n rospy.init_node(\"fri_example_joint_position_keyboard\")\n print(\"Getting robot state... \")\n\n def clean_shutdown():\n print(\"\\nExiting example.\")\n\n rospy.on_shutdown(clean_shutdown)\n\n map_keyboard()\n print(\"Done.\")\n\n\nif __name__ == '__main__':\n main()\n" }, { "path": "franka_interface/tests/test_zeroG.py", "content": "import rospy\nimport numpy as np\nfrom franka_interface import ArmInterface\n\n\"\"\"\n:info:\n This code is for testing the torque controller. The robot should be in zeroG mode, i.e.\n It should stay still when no external force is acting, but should be very compliant\n (almost no resistance) when pushed. This code sends zero torque command continuously,\n which means the internal gravity compensation is the only torque being sent to the \n robot. \n\n Test by pushing robot.\n\n DO NOT RUN THIS CODE WITH CUSTOM END-EFFECTOR UNLESS YOU KNOW WHAT YOU'RE DOING!\n\n WARNING: This code only works if the robot model is good! If you have installed custom\n end-effector, the gravity compensation may not be good unless you have incorporated\n the model to the FCI via Desk!!\n\"\"\"\n\nif __name__ == '__main__':\n rospy.init_node(\"test_zeroG\")\n r = ArmInterface() # create arm interface instance (see https://justagist.github.io/franka_ros_interface/DOC.html#arminterface for all available methods for ArmInterface() object)\n\n rospy.loginfo(\"Commanding...\\n\")\n\n r.move_to_neutral() # move robot to neutral pose\n\n rate = rospy.Rate(1000)\n\n joint_names = r.joint_names()\n\n while not rospy.is_shutdown():\n\n r.set_joint_torques(dict(zip(joint_names, [0.0]*7))) # send 0 torques\n rate.sleep()" }, { "path": "franka_moveit/CMakeLists.txt", "content": "cmake_minimum_required(VERSION 2.8.3)\nproject(franka_moveit)\n\nfind_package(catkin REQUIRED COMPONENTS\n panda_moveit_config\n)\n\ncatkin_package()\n\ninstall(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})\ninstall(DIRECTORY config DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})\n\ncatkin_python_setup()\ncatkin_install_python(PROGRAMS scripts/create_demo_planning_scene.py \n tests/test_scene_interface.py\n DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})\n" }, { "path": "franka_moveit/README.md", "content": "# franka_moveit\n\nSome helper classes which interfaces moveit with the Franka Emika Panda robot.\n\n### PandaMoveGroupInterface\n\n- Provides interface to control and plan motions using MoveIt in ROS.\n- Simple methods to plan and execute joint trajectories and cartesian path.\n- Provides easy reset and environment definition functionalities (See ExtendedPlanningSceneInterface below).\n\n### ExtendedPlanningSceneInterface\n\n- Easily define scene for robot motion planning (MoveIt plans will avoid defined obstacles if possible).\n" }, { "path": "franka_moveit/config/moveit_demo.rviz", "content": "Panels:\n - Class: rviz/Displays\n Help Height: 84\n Name: Displays\n Property Tree Widget:\n Expanded:\n - /MotionPlanning1\n - /MotionPlanning1/Scene Geometry1\n - /MotionPlanning1/Scene Robot1\n - /MotionPlanning1/Planning Request1\n - /MotionPlanning1/Planned Path1\n - /PlanningScene1\n - /PlanningScene1/Scene Geometry1\n Splitter Ratio: 0.7425600290298462\n Tree Height: 126\n - Class: rviz/Help\n Name: Help\n - Class: rviz/Views\n Expanded:\n - /Current View1\n Name: Views\n Splitter Ratio: 0.5\n - Class: rviz_visual_tools/RvizVisualToolsGui\n Name: RvizVisualToolsGui\nPreferences:\n PromptSaveOnExit: true\nToolbars:\n toolButtonStyle: 2\nVisualization Manager:\n Class: \"\"\n Displays:\n - Alpha: 0.5\n Cell Size: 1\n Class: rviz/Grid\n Color: 160; 160; 164\n Enabled: true\n Line Style:\n Line Width: 0.029999999329447746\n Value: Lines\n Name: Grid\n Normal Cell Count: 0\n Offset:\n X: 0\n Y: 0\n Z: 0\n Plane: XY\n Plane Cell Count: 10\n Reference Frame: \n Value: true\n - Class: rviz/MarkerArray\n Enabled: true\n Marker Topic: /rviz_visual_tools\n Name: MarkerArray\n Namespaces:\n {}\n Queue Size: 100\n Value: true\n - Acceleration_Scaling_Factor: 0.5\n Class: moveit_rviz_plugin/MotionPlanning\n Enabled: true\n Move Group Namespace: \"\"\n MoveIt_Allow_Approximate_IK: false\n MoveIt_Allow_External_Program: false\n MoveIt_Allow_Replanning: false\n MoveIt_Allow_Sensor_Positioning: false\n MoveIt_Goal_Tolerance: 0\n MoveIt_Planning_Attempts: 10\n MoveIt_Planning_Time: 5\n MoveIt_Use_Cartesian_Path: false\n MoveIt_Use_Constraint_Aware_IK: true\n MoveIt_Warehouse_Host: 127.0.0.1\n MoveIt_Warehouse_Port: 33829\n MoveIt_Workspace:\n Center:\n X: 0\n Y: 0\n Z: 0\n Size:\n X: 2\n Y: 2\n Z: 2\n Name: MotionPlanning\n Planned Path:\n Color Enabled: false\n Interrupt Display: false\n Links:\n All Links Enabled: true\n Expand Joint Details: false\n Expand Link Details: false\n Expand Tree: false\n Link Tree Style: Links in Alphabetic Order\n panda_link0:\n Alpha: 1\n Show Axes: false\n Show Trail: false\n Value: true\n panda_link1:\n Alpha: 1\n Show Axes: false\n Show Trail: false\n Value: true\n panda_link2:\n Alpha: 1\n Show Axes: false\n Show Trail: false\n Value: true\n panda_link3:\n Alpha: 1\n Show Axes: false\n Show Trail: false\n Value: true\n panda_link4:\n Alpha: 1\n Show Axes: false\n Show Trail: false\n Value: true\n panda_link5:\n Alpha: 1\n Show Axes: false\n Show Trail: false\n Value: true\n panda_link6:\n Alpha: 1\n Show Axes: false\n Show Trail: false\n Value: true\n panda_link7:\n Alpha: 1\n Show Axes: false\n Show Trail: false\n Value: true\n panda_link8:\n Alpha: 1\n Show Axes: false\n Show Trail: false\n world:\n Alpha: 1\n Show Axes: false\n Show Trail: false\n Loop Animation: false\n Robot Alpha: 0.5\n Robot Color: 150; 50; 150\n Show Robot Collision: false\n Show Robot Visual: true\n Show Trail: false\n State Display Time: 0.05 s\n Trail Step Size: 1\n Trajectory Topic: /move_group/display_planned_path\n Planning Metrics:\n Payload: 1\n Show Joint Torques: false\n Show Manipulability: false\n Show Manipulability Index: false\n Show Weight Limit: false\n TextHeight: 0.07999999821186066\n Planning Request:\n Colliding Link Color: 255; 0; 0\n Goal State Alpha: 1\n Goal State Color: 250; 128; 0\n Interactive Marker Size: 0\n Joint Violation Color: 255; 0; 255\n Planning Group: panda_arm\n Query Goal State: true\n Query Start State: false\n Show Workspace: false\n Start State Alpha: 1\n Start State Color: 0; 255; 0\n Planning Scene Topic: /planning_scene\n Robot Description: robot_description\n Scene Geometry:\n Scene Alpha: 0.8999999761581421\n Scene Color: 50; 230; 50\n Scene Display Time: 0.20000000298023224\n Show Scene Geometry: false\n Voxel Coloring: Z-Axis\n Voxel Rendering: Occupied Voxels\n Scene Robot:\n Attached Body Color: 150; 50; 150\n Links:\n All Links Enabled: true\n Expand Joint Details: false\n Expand Link Details: false\n Expand Tree: false\n Link Tree Style: Links in Alphabetic Order\n panda_link0:\n Alpha: 1\n Show Axes: false\n Show Trail: false\n Value: true\n panda_link1:\n Alpha: 1\n Show Axes: false\n Show Trail: false\n Value: true\n panda_link2:\n Alpha: 1\n Show Axes: false\n Show Trail: false\n Value: true\n panda_link3:\n Alpha: 1\n Show Axes: false\n Show Trail: false\n Value: true\n panda_link4:\n Alpha: 1\n Show Axes: false\n Show Trail: false\n Value: true\n panda_link5:\n Alpha: 1\n Show Axes: false\n Show Trail: false\n Value: true\n panda_link6:\n Alpha: 1\n Show Axes: false\n Show Trail: false\n Value: true\n panda_link7:\n Alpha: 1\n Show Axes: false\n Show Trail: false\n Value: true\n panda_link8:\n Alpha: 1\n Show Axes: false\n Show Trail: false\n world:\n Alpha: 1\n Show Axes: false\n Show Trail: false\n Robot Alpha: 1\n Show Robot Collision: false\n Show Robot Visual: true\n Value: true\n Velocity_Scaling_Factor: 0.5\n - Class: moveit_rviz_plugin/PlanningScene\n Enabled: true\n Move Group Namespace: \"\"\n Name: PlanningScene\n Planning Scene Topic: move_group/monitored_planning_scene\n Robot Description: robot_description\n Scene Geometry:\n Scene Alpha: 1\n Scene Color: 170; 255; 255\n Scene Display Time: 0.20000000298023224\n Show Scene Geometry: true\n Voxel Coloring: Z-Axis\n Voxel Rendering: Occupied Voxels\n Scene Robot:\n Attached Body Color: 150; 50; 150\n Links:\n All Links Enabled: true\n Expand Joint Details: false\n Expand Link Details: false\n Expand Tree: false\n Link Tree Style: Links in Alphabetic Order\n panda_link0:\n Alpha: 1\n Show Axes: false\n Show Trail: false\n Value: true\n panda_link1:\n Alpha: 1\n Show Axes: false\n Show Trail: false\n Value: true\n panda_link2:\n Alpha: 1\n Show Axes: false\n Show Trail: false\n Value: true\n panda_link3:\n Alpha: 1\n Show Axes: false\n Show Trail: false\n Value: true\n panda_link4:\n Alpha: 1\n Show Axes: false\n Show Trail: false\n Value: true\n panda_link5:\n Alpha: 1\n Show Axes: false\n Show Trail: false\n Value: true\n panda_link6:\n Alpha: 1\n Show Axes: false\n Show Trail: false\n Value: true\n panda_link7:\n Alpha: 1\n Show Axes: false\n Show Trail: false\n Value: true\n panda_link8:\n Alpha: 1\n Show Axes: false\n Show Trail: false\n world:\n Alpha: 1\n Show Axes: false\n Show Trail: false\n Robot Alpha: 1\n Show Robot Collision: false\n Show Robot Visual: true\n Value: true\n Enabled: true\n Global Options:\n Background Color: 48; 48; 48\n Default Light: true\n Fixed Frame: panda_link0\n Frame Rate: 30\n Name: root\n Tools:\n - Class: rviz/Interact\n Hide Inactive Objects: true\n - Class: rviz/MoveCamera\n - Class: rviz/Select\n - Class: rviz_visual_tools/KeyTool\n Value: true\n Views:\n Current:\n Class: rviz/XYOrbit\n Distance: 2.827594041824341\n Enable Stereo Rendering:\n Stereo Eye Separation: 0.05999999865889549\n Stereo Focal Distance: 1\n Swap Stereo Eyes: false\n Value: false\n Focal Point:\n X: 0.11356700211763382\n Y: 0.10592000186443329\n Z: 2.2351800055275817e-7\n Focal Shape Fixed Size: true\n Focal Shape Size: 0.05000000074505806\n Invert Z Axis: false\n Name: Current View\n Near Clip Distance: 0.009999999776482582\n Pitch: 0.27479690313339233\n Target Frame: panda_link0\n Value: XYOrbit (rviz)\n Yaw: 5.819954872131348\n Saved: ~\nWindow Geometry:\n Displays:\n collapsed: false\n Height: 759\n Help:\n collapsed: false\n Hide Left Dock: false\n Hide Right Dock: false\n MotionPlanning:\n collapsed: false\n MotionPlanning - Trajectory Slider:\n collapsed: false\n QMainWindow State: 000000ff00000000fd000000010000000000000274000002a1fc020000000dfb000000100044006900730070006c006100790073010000003b0000010d000000c700fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a00560069006500770073000000026d000000b5000000a000fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb0000002e004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d00200053006c00690064006500720000000000ffffffff0000000000000000fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e0067010000014e0000018e0000018300fffffffb00000024005200760069007a00560069007300750061006c0054006f006f006c007300470075006900000004bd0000008d0000003f00fffffffb000000120020002d00200053006c00690064006500720000000000ffffffff0000000000000000fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e0067010000034f000001fb0000000000000000fb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e00670100000308000001760000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000003f00ffffff00000326000002a100000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000\n RvizVisualToolsGui:\n collapsed: false\n Views:\n collapsed: false\n Width: 1440\n X: 297\n Y: 101\n" }, { "path": "franka_moveit/launch/demo_moveit.launch", "content": "\n\n \n \n \n \n\n \n \n \n \n\n \n \n \n\n \n \n \n \n \n\n \n \n\n\n \n\n \n\n\n\n \n\n \n\n\n \n\n \n\n\n\n\n" }, { "path": "franka_moveit/package.xml", "content": "\n\n franka_moveit\n 0.7.1\n \n franka_moveit package wrapping franka_moveit_config using franka_ros_interface.\n \n\n \n Saif Sidhik\n \n Apache 2.0\n Saif Sidhik\n\n catkin\n panda_moveit_config\n panda_moveit_config\n\n\n" }, { "path": "franka_moveit/scripts/create_demo_planning_scene.py", "content": "#!/usr/bin/env python\n\n# /***************************************************************************\n\n# \n# @package: franka_moveit\n# @metapackage: franka_ros_interface\n# @author: Saif Sidhik \n# \n\n# **************************************************************************/\n\n# /***************************************************************************\n# Copyright (c) 2019-2021, Saif Sidhik\n \n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n\n# http://www.apache.org/licenses/LICENSE-2.0\n\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n# **************************************************************************/\n\nimport sys\nimport rospy\nimport moveit_commander\n\nfrom franka_moveit import ExtendedPlanningSceneInterface\nfrom franka_moveit.utils import create_pose_stamped_msg\n\n\"\"\"\nA script for creating a simple environment as a PlanningScene. This script runs \nby default when interface.launch is started, but can be disabled using argument.\n\"\"\"\n\nIRLab_workspace = [\n {\n 'name': 'back_wall',\n 'pose': create_pose_stamped_msg(position = [-0.57,0.0,0.5], orientation = [1,0,0,0], frame = 'panda_link0'),\n 'size': [0.1,1.8,1]\n },\n {\n 'name': 'side_wall',\n 'pose': create_pose_stamped_msg(position = [-0.3,-0.85,0.5], orientation = [1,0,0,0], frame = 'panda_link0'),\n 'size': [0.6,0.1,1]\n },\n {\n 'name': 'table',\n 'pose': create_pose_stamped_msg(position = [0.45,-0.0,0], orientation = [1,0,0,0], frame = 'panda_link0'),\n 'size': [2,1.8,0.02]\n },\n {\n 'name': 'controller_box',\n 'pose': create_pose_stamped_msg(position = [-0.37,0.55,0.08], orientation = [1,0,0,0], frame = 'panda_link0'),\n 'size': [0.4,0.6,0.16]\n },\n {\n 'name': 'equipment_box',\n 'pose': create_pose_stamped_msg(position = [-0.35,-0.68,0.17], orientation = [1,0,0,0], frame = 'panda_link0'),\n 'size': [0.46,0.4,0.34]\n }\n ]\n\n\n\ndef main():\n try:\n rospy.loginfo(\"Creating Demo Planning Scene\")\n scene = ExtendedPlanningSceneInterface()\n\n rospy.sleep(1) # ----- Not having this delay sometimes caused failing to create some boxes\n\n for config in IRLab_workspace:\n\n rospy.loginfo(\"-- Creating object: {}..\".format(config['name']))\n success = scene.add_box(**config)\n rospy.loginfo(\"------ {}\".format(\"success\" if success else \"FAILED!\"))\n\n rospy.loginfo(\"Created Demo Planning Scene.\")\n\n except rospy.ROSInterruptException:\n return\n except KeyboardInterrupt:\n return\n\nif __name__ == '__main__':\n rospy.init_node('simple_scene_creator',\n anonymous=True)\n\n moveit_commander.roscpp_initialize(sys.argv)\n\n main()\n\n" }, { "path": "franka_moveit/setup.py", "content": "#!/usr/bin/env python\nfrom distutils.core import setup\nfrom catkin_pkg.python_setup import generate_distutils_setup\n\nd = generate_distutils_setup()\nd['packages'] = ['franka_moveit']\nd['package_dir'] = {'': 'src'}\n\nsetup(**d)\n" }, { "path": "franka_moveit/src/franka_moveit/__init__.py", "content": "# /***************************************************************************\n# Copyright (c) 2019-2021, Saif Sidhik\n \n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n\n# http://www.apache.org/licenses/LICENSE-2.0\n\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n# **************************************************************************/\nfrom .extended_planning_scene_interface import ExtendedPlanningSceneInterface\nfrom .movegroup_interface import PandaMoveGroupInterface\nfrom . import utils\n" }, { "path": "franka_moveit/src/franka_moveit/extended_planning_scene_interface.py", "content": "# /***************************************************************************\n\n# \n# @package: franka_moveit\n# @metapackage: franka_ros_interface\n# @author: Saif Sidhik \n# \n\n# **************************************************************************/\n\n# /***************************************************************************\n# Copyright (c) 2019-2021, Saif Sidhik\n \n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n\n# http://www.apache.org/licenses/LICENSE-2.0\n\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n# **************************************************************************/\nimport rospy\nimport moveit_commander\n\nclass ExtendedPlanningSceneInterface(moveit_commander.PlanningSceneInterface):\n \"\"\"\n .. note:: For other available methods for planning scene interface, refer `PlanningSceneInterface `_.\n \"\"\"\n def __init__(self):\n\n moveit_commander.PlanningSceneInterface.__init__(self)\n\n\n def add_box(self, name, pose, size, timeout = 5):\n \"\"\"\n Add object to scene and check if it is created.\n\n :param name: name of object\n :param pose: desired pose for the box (Use :py:func:`franka_moveit.utils.create_pose_stamped_msg`)\n :param size: size of the box\n :param timeout: time in sec to wait while checking if box is created \n\n :type name: str\n :type pose: geometry_msgs.msg.PoseStamped\n :type size: [float] (len 3)\n :type timeout: float\n \"\"\"\n\n moveit_commander.PlanningSceneInterface.add_box(self, name = name, pose = pose, size=size)\n return self._wait_for_state_update(object_name = name, object_is_known=True, timeout=timeout)\n\n\n\n def _wait_for_state_update(self, object_name, object_is_known=False, object_is_attached=False, timeout=5):\n\n start = rospy.get_time()\n while (rospy.get_time() - start < timeout) and not rospy.is_shutdown():\n\n attached_objects = self.get_attached_objects([object_name])\n is_attached = len(list(attached_objects.keys())) > 0\n\n is_known = object_name in self.get_known_object_names()\n\n if (object_is_attached == is_attached) and (object_is_known == is_known):\n return True\n\n rospy.sleep(0.1)\n return False\n\n def remove_box(self, box_name, timeout=5):\n \"\"\"\n Remove box from scene.\n\n :param box_name: name of object\n :param timeout: time in sec to wait while checking if box is created \n\n :type box_name: str\n :type timeout: float\n\n \"\"\"\n self.remove_world_object(box_name)\n\n return self._wait_for_state_update(object_name = box_name, object_is_attached=False, object_is_known=False, timeout=timeout)\n\n\n\n\nif __name__ == '__main__':\n scene = ExtendedPlanningSceneInterface()\n" }, { "path": "franka_moveit/src/franka_moveit/movegroup_interface.py", "content": "# /***************************************************************************\n\n# \n# @package: franka_moveit\n# @metapackage: franka_ros_interface\n# @author: Saif Sidhik \n# \n\n# **************************************************************************/\n\n# /***************************************************************************\n# Copyright (c) 2019-2021, Saif Sidhik\n \n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n\n# http://www.apache.org/licenses/LICENSE-2.0\n\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n# **************************************************************************/\n\nimport sys\nimport copy\nimport rospy\nimport moveit_commander\nimport moveit_msgs.msg\nimport geometry_msgs.msg\nfrom moveit_commander.conversions import pose_to_list\nfrom franka_moveit import ExtendedPlanningSceneInterface\n\n\ndef all_close(goal, actual, tolerance):\n \"\"\"\n Convenience method for testing if a list of values are within a tolerance of their counterparts in another list\n :param: goal A list of floats, a Pose or a PoseStamped\n :param: actual A list of floats, a Pose or a PoseStamped\n :param: tolerance A float\n :rtype: bool\n \"\"\"\n if isinstance(goal, list):\n for index, g in enumerate(goal):\n if abs(actual[index] - g) > tolerance:\n return False\n\n elif isinstance(goal, geometry_msgs.msg.PoseStamped):\n return all_close(goal.pose, actual.pose, tolerance)\n\n elif isinstance(goal, geometry_msgs.msg.Pose):\n return all_close(pose_to_list(goal), pose_to_list(actual), tolerance)\n\n return True\n\n\nclass PandaMoveGroupInterface:\n\n def __init__(self, use_panda_hand_link=False):\n\n try:\n rospy.get_param(\"/robot_description_semantic\")\n except KeyError:\n rospy.loginfo((\"Moveit server does not seem to be running.\"))\n raise Exception\n except (socket.error, socket.gaierror):\n print (\"Failed to connect to the ROS parameter server!\\n\"\n \"Please check to make sure your ROS networking is \"\n \"properly configured:\\n\")\n sys.exit()\n\n moveit_commander.roscpp_initialize(sys.argv)\n\n self._robot = moveit_commander.RobotCommander()\n\n self._scene = ExtendedPlanningSceneInterface()\n\n self._arm_group = moveit_commander.MoveGroupCommander(\"panda_arm\")\n\n try:\n rospy.get_param(\"/franka_gripper/robot_ip\")\n self._gripper_group = moveit_commander.MoveGroupCommander(\"hand\")\n except KeyError:\n rospy.loginfo((\"PandaMoveGroupInterface: could not detect gripper.\"))\n self._gripper_group = None\n except (socket.error, socket.gaierror):\n print (\"Failed to connect to the ROS parameter server!\\n\"\n \"Please check to make sure your ROS networking is \"\n \"properly configured:\\n\")\n sys.exit()\n\n self._display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path',\n moveit_msgs.msg.DisplayTrajectory,\n queue_size=20)\n\n if use_panda_hand_link and self._gripper_group is not None:\n self._default_ee = 'panda_hand'\n else:\n self._default_ee = 'panda_link8'\n self._arm_group.set_end_effector_link(self._default_ee)\n\n rospy.logdebug(\"PandaMoveGroupInterface: Setting default EE link to '{}' \"\n \"Use group.set_end_effector_link() method to change default EE.\".format(self._default_ee))\n\n self._arm_group.set_max_velocity_scaling_factor(0.3)\n\n\n @property\n def robot_state_interface(self):\n \"\"\"\n :getter: The RobotCommander instance of this object\n :type: moveit_commander.RobotCommander\n \n .. note:: For available methods for RobotCommander, refer `RobotCommander `_.\n \n \"\"\"\n return self._robot\n \n @property\n def scene(self):\n \"\"\"\n :getter: The PlanningSceneInterface instance for this robot. This is an interface\n to the world surrounding the robot\n :type: franka_moveit.ExtendedPlanningSceneInterface\n\n .. note:: For other available methods for planning scene interface, refer `PlanningSceneInterface `_. \n\n \"\"\"\n return self._scene\n\n @property\n def arm_group(self):\n \"\"\"\n :getter: The MoveGroupCommander instance of this object. This is an interface\n to one group of joints. In this case the group is the joints in the Panda\n arm. This interface can be used to plan and execute motions on the Panda.\n :type: moveit_commander.MoveGroupCommander\n\n .. note:: For available methods for movegroup, refer `MoveGroupCommander `_. \n \"\"\"\n return self._arm_group\n\n @property\n def gripper_group(self):\n \"\"\"\n :getter: The MoveGroupCommander instance of this object. This is an interface\n to one group of joints. In this case the group is the joints in the Panda\n gripper. This interface can be used to plan and execute motions of the gripper.\n :type: moveit_commander.MoveGroupCommander\n\n .. note:: For available methods for movegroup, refer `MoveGroupCommander `_. \n \"\"\"\n return self._gripper_group\n\n def go_to_joint_positions(self, positions, wait = True, tolerance = 0.005):\n \"\"\"\n :return: status of joint motion plan execution\n :rtype: bool\n\n :param positions: target joint positions (ordered)\n :param wait: if True, function will wait for trajectory execution to complete\n :param tolerance: maximum error in final position for each joint to consider\n task a success\n\n :type positions: [double]\n :type wait: bool\n :type tolerance: double\n \"\"\"\n self._arm_group.clear_pose_targets()\n\n rospy.logdebug(\"Starting positions: {}\".format(self._arm_group.get_current_joint_values()))\n\n self._arm_group.go(positions[:7], wait = wait)\n\n if len(positions) > 7:\n self._gripper_group.go(positions[7:], wait = wait)\n\n if wait:\n self._arm_group.stop()\n gripper_tolerance = True\n if len(positions)> 7: \n self._gripper_group.stop()\n gripper_tolerance = all_close(positions[7:], self._gripper_group.get_current_joint_values(), tolerance)\n return all_close(positions[:7], self._arm_group.get_current_joint_values(), tolerance) and gripper_tolerance\n\n return True\n\n def go_to_cartesian_pose(self, pose, ee_link=\"\", wait=True):\n \"\"\"\n Plan and execute a cartesian path to reach a target by avoiding obstacles in the scene.\n For planning through multiple points, use func:`self._arm_group.set_pose_targets`.\n\n :param pose: The cartesian pose to be reached. \n (Use :func:`franka_moveit.utils.create_pose_msg` for creating pose messages easily)\n :type pose: geomentry_msgs.msg.Pose\n :param ee_link: name of end-effector link to be used; uses currently set value by default\n :type ee_link: str, optional\n :param wait: if set to True, blocks till execution is complete; defaults to True\n :type wait: bool\n \"\"\"\n self._arm_group.set_pose_target(pose, end_effector_link=ee_link)\n self._arm_group.go(wait=wait)\n\n\n def plan_cartesian_path(self, poses):\n \"\"\"\n Plan cartesian path using the provided list of poses.\n\n :param poses: The cartesian poses to be achieved in sequence. \n (Use :func:`franka_moveit.utils.create_pose_msg` for creating pose messages easily)\n :type poses: [geomentry_msgs.msg.Pose]\n\n :return: the actual RobotTrajectory (can be used for :py:meth:`execute_plan`), a fraction of how much of the path was followed\n :rtype: [RobotTrajectory, float (0,1)]\n\n .. note:: This method will NOT make the robot avoid obstacles defined in scene. Use func:`go_to_cartesian_pose` for moving to target pose and avoiding obstacles.\n \"\"\"\n waypoints = []\n for pose in poses:\n waypoints.append(copy.deepcopy(pose))\n\n plan, fraction = self._arm_group.compute_cartesian_path(waypoints, 0.01, 0.0)\n\n return plan, fraction\n\n def set_velocity_scale(self, value, group = \"arm\"):\n \"\"\"\n Set the max velocity scale for executing planned motion.\n \n :param value: scale value (allowed (0,1] )\n :type value: float\n \"\"\"\n if group == \"arm\":\n self._arm_group.set_max_velocity_scaling_factor(value)\n elif group == \"gripper\":\n self._gripper_group.set_max_velocity_scaling_factor(value)\n else:\n raise ValueError(\"PandaMoveGroupInterface: Invalid group specified!\")\n rospy.logdebug(\"PandaMoveGroupInterface: Setting velocity scale to {}\".format(value))\n\n def plan_joint_path(self, joint_position):\n \"\"\"\n :return: RobotTrajectory plan for executing joint trajectory (can be used for :py:meth:`execute_plan`)\n\n :param joint_position: target joint positions\n :type joint_position: [float]*7\n \"\"\"\n return self._arm_group.plan(joint_position)\n\n\n def close_gripper(self, wait = False):\n \"\"\"\n Close gripper. (Using named states defined in urdf.)\n\n :param wait: if set to True, blocks till execution is complete; defaults to True\n :type wait: bool\n\n .. note:: If this named state is not found, your ros environment is\n probably not using the right panda_moveit_config package. Ensure\n that sourced package is from this repo -->\n https://github.com/justagist/panda_moveit_config\n\n \"\"\"\n self._gripper_group.set_named_target(\"close\")\n return self._gripper_group.go(wait = wait)\n\n def open_gripper(self, wait = False):\n \"\"\"\n Open gripper. (Using named states defined in urdf)\n\n :param wait: if set to True, blocks till execution is complete\n :type wait: bool\n\n .. note:: If this named state is not found, your ros environment is\n probably not using the right panda_moveit_config package. Ensure\n that sourced package is from this repo -->\n https://github.com/justagist/panda_moveit_config.\n \"\"\"\n self._gripper_group.set_named_target(\"open\")\n return self._gripper_group.go(wait = wait)\n\n def display_trajectory(self, plan):\n \"\"\"\n Display planned trajectory in RViz. Rviz should be open and Trajectory\n display should be listening to the appropriate trajectory topic.\n\n :param plan: the plan to be executed (from :func:`plan_joint_path` or\n :py:meth:`plan_cartesian_path`)\n \"\"\"\n display_trajectory = moveit_msgs.msg.DisplayTrajectory()\n display_trajectory.trajectory_start = self._robot.get_current_state()\n display_trajectory.trajectory.append(plan)\n # Publish\n self._display_trajectory_publisher.publish(display_trajectory)\n\n def move_to_neutral(self, wait = True):\n \"\"\"\n Send arm group to neutral pose defined using named state in urdf.\n\n :param wait: if set to True, blocks till execution is complete\n :type wait: bool\n \"\"\"\n self._arm_group.set_named_target(\"ready\")\n return self._arm_group.go(wait = wait)\n\n def execute_plan(self, plan, group = \"arm\", wait = True):\n \"\"\"\n Execute the planned trajectory \n\n :param plan: The plan to be executed (from :py:meth:`plan_joint_path` or\n :py:meth:`plan_cartesian_path`)\n :param group: The name of the move group (default \"arm\" for robot; use \"hand\" \n for gripper group)\n :type group: str\n :param wait: if set to True, blocks till execution is complete\n :type wait: bool\n \"\"\"\n if group == \"arm\":\n self._arm_group.execute(plan, wait = wait)\n elif group == \"gripper\":\n self._gripper_group.execute(plan, wait = wait)\n else:\n raise ValueError(\"PandaMoveGroupInterface: Invalid group specified!\")\n\n\n\n\n\n\n \n\n\n\n\n \n \n\n\n" }, { "path": "franka_moveit/src/franka_moveit/utils.py", "content": "\nimport geometry_msgs.msg\nimport quaternion\n\ndef create_pose_msg(position, orientation):\n \"\"\"\n Create Pose message using the provided position and orientation\n\n :return: Pose message for the give end-effector position and orientation\n :rtype: geometry_msgs.msg.Pose\n :param position: End-effector position in base frame of the robot\n :type position: [float]*3\n :param orientation: orientation quaternion of end-effector in base frame\n :type orientation: quaternion.quaternion (OR) [float] size 4: (w,x,y,z)\n \"\"\"\n pose = geometry_msgs.msg.Pose()\n\n pose.position.x = position[0]\n pose.position.y = position[1]\n pose.position.z = position[2]\n\n if isinstance(orientation,quaternion.quaternion):\n pose.orientation.x = orientation.x\n pose.orientation.y = orientation.y\n pose.orientation.z = orientation.z\n pose.orientation.w = orientation.w\n else:\n pose.orientation.x = orientation[1]\n pose.orientation.y = orientation[2]\n pose.orientation.z = orientation[3]\n pose.orientation.w = orientation[0]\n\n return pose\n\ndef create_pose_stamped_msg(position, orientation, frame = \"world\"):\n \"\"\"\n Create PoseStamped message using the provided position and orientation\n\n :return: Pose message for the give end-effector position and orientation\n :rtype: geometry_msgs.msg.Pose\n :param position: End-effector position in base frame of the robot\n :type position: [float]*3\n :param orientation: orientation quaternion of end-effector in base frame\n :type orientation: quaternion.quaternion (OR) [float] size 4: (w,x,y,z)\n :param frame: Name of the parent frame\n :type frame: str\n \"\"\"\n pose = geometry_msgs.msg.PoseStamped()\n\n pose.header.frame_id = frame\n\n pose.pose = create_pose_msg(position, orientation)\n\n return pose\n " }, { "path": "franka_moveit/tests/test_movegroup_interface.py", "content": "import rospy\nfrom franka_moveit import PandaMoveGroupInterface\n\nif __name__ == '__main__':\n \n\n rospy.init_node(\"test_moveit\")\n mvt = PandaMoveGroupInterface()\n\n g = mvt.gripper_group\n r = mvt.arm_group\n\n\n" }, { "path": "franka_moveit/tests/test_scene_interface.py", "content": "#!/usr/bin/env python\n\nimport sys\nimport rospy\nimport moveit_commander\nfrom builtins import input\n\nfrom franka_moveit import ExtendedPlanningSceneInterface\nfrom franka_moveit.utils import create_pose_stamped_msg\n\n\"\"\"\n Test if sceneinterface works. Use rviz and add PlanningScene visual while running the interface.\n\"\"\"\n\nobjects = [\n {\n 'name': 'box1',\n 'pose': create_pose_stamped_msg(position = [0,0,0], orientation = [1,0,0,0], frame = \"world\"), \n 'size': [0.1,0.1,0.1]\n } ,\n {\n 'name': 'box2',\n 'pose': create_pose_stamped_msg(position = [0.5,0,0], orientation = [1,0,0,0], frame = \"world\"),\n 'size': [0.2,0.2,0.2]\n },\n {\n 'name': 'box3',\n 'pose': create_pose_stamped_msg(position = [0.4,-0.2,0.5], orientation = [1,0,0,0], frame = \"world\"),\n 'size': [0.1,0.1,0.1]\n }\n ]\n\n\n\ndef main():\n\n print(\"============ Starting test ...\")\n rospy.sleep(5.)\n try:\n input(\"============ Press `Enter` to begin the tutorial by setting up the scene_interface (press ctrl-d to exit) ...\")\n \n scene = ExtendedPlanningSceneInterface()\n\n input(\"============ Press `Enter` to add objects to the planning scene ...\")\n \n for config in objects:\n scene.add_box(**config)\n\n input(\"============ Press `Enter` to remove the boxes from the planning scene ...\")\n \n for config in objects:\n scene.remove_box(config['name'])\n\n except rospy.ROSInterruptException:\n return\n except KeyboardInterrupt:\n return\n\nif __name__ == '__main__':\n rospy.init_node('simple_scene_tester',\n anonymous=True)\n\n moveit_commander.roscpp_initialize(sys.argv)\n\n main()\n\n" }, { "path": "franka_ros_controllers/CMakeLists.txt", "content": "cmake_minimum_required(VERSION 2.8.3)\nproject(franka_ros_controllers)\n\nset(CMAKE_BUILD_TYPE Release)\n\nset(CMAKE_CXX_STANDARD 14)\nset(CMAKE_CXX_STANDARD_REQUIRED ON)\n\nfind_package(catkin REQUIRED COMPONENTS\n controller_interface\n dynamic_reconfigure\n franka_hw\n geometry_msgs\n franka_core_msgs\n hardware_interface\n message_generation\n pluginlib\n realtime_tools\n roscpp\n rospy\n)\n\nfind_package(Eigen3 REQUIRED)\nfind_package(Franka 0.5.0 REQUIRED)\n\ngenerate_dynamic_reconfigure_options(\n cfg/joint_controller_params.cfg\n)\n\ncatkin_package(\n LIBRARIES franka_ros_controllers\n CATKIN_DEPENDS\n controller_interface\n dynamic_reconfigure\n franka_hw\n geometry_msgs\n franka_core_msgs\n hardware_interface\n message_runtime\n pluginlib\n realtime_tools\n roscpp\n DEPENDS Franka\n)\n\nadd_library(franka_ros_controllers\n src/effort_joint_impedance_controller.cpp\n src/position_joint_position_controller.cpp\n src/effort_joint_position_controller.cpp\n src/effort_joint_torque_controller.cpp\n src/velocity_joint_velocity_controller.cpp\n)\n\nadd_dependencies(franka_ros_controllers\n ${${PROJECT_NAME}_EXPORTED_TARGETS}\n ${catkin_EXPORTED_TARGETS}\n ${PROJECT_NAME}_gencfg\n)\n\ntarget_link_libraries(franka_ros_controllers PUBLIC\n ${Franka_LIBRARIES}\n ${catkin_LIBRARIES}\n)\n\ntarget_include_directories(franka_ros_controllers SYSTEM PUBLIC\n ${Franka_INCLUDE_DIRS}\n ${EIGEN3_INCLUDE_DIRS}\n ${catkin_INCLUDE_DIRS}\n)\ntarget_include_directories(franka_ros_controllers PUBLIC\n include\n)\n\n## Installation\ninstall(TARGETS franka_ros_controllers\n ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}\n LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}\n RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}\n)\n\ninstall(DIRECTORY config\n DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}\n)\ninstall(FILES controller_plugins.xml\n DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}\n)\n# catkin_install_python(\n# PROGRAMS scripts/interactive_marker.py\n# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}\n# )\n\n## Tools\ninclude(${CMAKE_CURRENT_LIST_DIR}/../cmake/ClangTools.cmake OPTIONAL\n RESULT_VARIABLE CLANG_TOOLS\n)\nif(CLANG_TOOLS)\n file(GLOB_RECURSE SOURCES ${CMAKE_CURRENT_SOURCE_DIR}/src/*.cpp)\n file(GLOB_RECURSE HEADERS\n ${CMAKE_CURRENT_SOURCE_DIR}/include/*.h\n ${CMAKE_CURRENT_SOURCE_DIR}/src/*.h\n )\n add_format_target(franka_ros_controllers FILES ${SOURCES} ${HEADERS})\n add_tidy_target(franka_ros_controllers\n FILES ${SOURCES}\n DEPENDS franka_ros_controllers\n )\nendif()\n" }, { "path": "franka_ros_controllers/README.md", "content": "# franka_ros_controllers\n\nControllers for the Franka Emika Panda robot using ROS topics.\n\n### Features:\n\n- Includes joint position, joint velocity, and joint effort (direct torque, indirect position and impedance) controllers that can be controlled directly through ROS topic \n- Same topic is used for all controllers; different keyword required in the ROS message for each controller (see *set_joint_positions*, *set_joint_velocities*, etc implemented in *franka_ros_interface/franka_interface/arm.py*)\n- Controller gains and other parameters can be controlled using dynamic reconfiguration or service calls (or using python API: *ControllerParamConfigClient* from *franka_ros_interface/franka_tools*). Default values can be set in the config file.\n\n" }, { "path": "franka_ros_controllers/cfg/joint_controller_params.cfg", "content": "#!/usr/bin/env python\nPACKAGE = \"franka_ros_controllers\"\n\nfrom dynamic_reconfigure.parameter_generator_catkin import *\n\ngen = ParameterGenerator()\n\n\"\"\"\n Joint Position Controller (position_joint_position_controller) smoothing parameter. Increasing the value gives higher smoothing (smaller change in target position per controller update step).\n\"\"\"\ngen.add(\"position_joint_delta_filter\", double_t, 0, \"Filter parameter that dictates how smoothly subsequent joint position commands are smoothed. Also responsible for speed of joint position execution.\", 0.3, 0.1, 1.0) # (default, min, max)\ngen.add(\"velocity_joint_delta_filter\", double_t, 0, \"Filter parameter that dictates how smoothly subsequent joint velocity commands are smoothed. Also responsible for acceleration of joints.\", 0.3, 0.1, 1.0) # (default, min, max)\n\n\n\"\"\"\n Joint positions controller gains\n\"\"\"\ncontroller_gains = gen.add_group(\"Controller_Gains\")\n\ncontroller_gains.add(\"j1_k\", double_t, 0, \"Stiffness parameter of joint 1\", 1200, 200, 1500) # (default, min, max)\ncontroller_gains.add(\"j2_k\", double_t, 0, \"Stiffness parameter of joint 2\", 1000, 200, 1500)\ncontroller_gains.add(\"j3_k\", double_t, 0, \"Stiffness parameter of joint 3\", 1000, 200, 1500)\ncontroller_gains.add(\"j4_k\", double_t, 0, \"Stiffness parameter of joint 4\", 800, 200, 1500)\ncontroller_gains.add(\"j5_k\", double_t, 0, \"Stiffness parameter of joint 5\", 300, 100, 1000)\ncontroller_gains.add(\"j6_k\", double_t, 0, \"Stiffness parameter of joint 6\", 200, 75, 500)\ncontroller_gains.add(\"j7_k\", double_t, 0, \"Stiffness parameter of joint 7\", 50, 20, 200)\n\ncontroller_gains.add(\"j1_d\", double_t, 0, \"Damping parameter of joint 1\", 50, 0, 200) # (default, min, max)\ncontroller_gains.add(\"j2_d\", double_t, 0, \"Damping parameter of joint 2\", 50, 0, 200)\ncontroller_gains.add(\"j3_d\", double_t, 0, \"Damping parameter of joint 3\", 50, 0, 200)\ncontroller_gains.add(\"j4_d\", double_t, 0, \"Damping parameter of joint 4\", 20, 0, 200)\ncontroller_gains.add(\"j5_d\", double_t, 0, \"Damping parameter of joint 5\", 20, 0, 200)\ncontroller_gains.add(\"j6_d\", double_t, 0, \"Damping parameter of joint 6\", 20, 0, 200)\ncontroller_gains.add(\"j7_d\", double_t, 0, \"Damping parameter of joint 7\", 10, 0, 200)\n\nexit(gen.generate(PACKAGE, \"controller_configurations\", \"joint_controller_params\"))\n" }, { "path": "franka_ros_controllers/config/ros_controllers.yaml", "content": "position_joint_position_controller:\n type: franka_ros_controllers/PositionJointPositionController\n joint_names:\n - panda_joint1\n - panda_joint2\n - panda_joint3\n - panda_joint4\n - panda_joint5\n - panda_joint6\n - panda_joint7\n\nvelocity_joint_velocity_controller:\n type: franka_ros_controllers/VelocityJointVelocityController\n joint_names:\n - panda_joint1\n - panda_joint2\n - panda_joint3\n - panda_joint4\n - panda_joint5\n - panda_joint6\n - panda_joint7\n\n\neffort_joint_impedance_controller:\n type: franka_ros_controllers/EffortJointImpedanceController\n arm_id: panda\n joint_names:\n - panda_joint1\n - panda_joint2\n - panda_joint3\n - panda_joint4\n - panda_joint5\n - panda_joint6\n - panda_joint7\n k_gains: # if changing the default values, remember to change in the cfg file as well\n - 1200.0\n - 1000.0\n - 1000.0\n - 800.0\n - 300.0\n - 200.0\n - 50.0\n d_gains:\n - 50.0\n - 50.0\n - 50.0\n - 20.0\n - 20.0\n - 20.0\n - 10.0\n publish_rate: 30.0\n coriolis_factor: 1.0\n\neffort_joint_position_controller:\n type: franka_ros_controllers/EffortJointPositionController\n arm_id: panda\n joint_names:\n - panda_joint1\n - panda_joint2\n - panda_joint3\n - panda_joint4\n - panda_joint5\n - panda_joint6\n - panda_joint7\n k_gains:\n - 1200.0\n - 1000.0\n - 1000.0\n - 800.0\n - 300.0\n - 200.0\n - 50.0\n d_gains:\n - 50.0\n - 50.0\n - 50.0\n - 20.0\n - 20.0\n - 20.0\n - 10.0\n publish_rate: 30.0\n\neffort_joint_torque_controller:\n type: franka_ros_controllers/EffortJointTorqueController\n arm_id: panda\n compensate_coriolis: false\n joint_names:\n - panda_joint1\n - panda_joint2\n - panda_joint3\n - panda_joint4\n - panda_joint5\n - panda_joint6\n - panda_joint7\n\n" }, { "path": "franka_ros_controllers/controller_plugins.xml", "content": "\n \n \n Controller for commanding joint position and velocity of the Franka arm using impedance control (torque)\n \n \n \n \n Controller for commanding absolute joint positions of the Franka arm\n \n \n \n \n Controller for commanding joint position of the Franka arm using effort control (torque)\n \n \n \n \n Controller for commanding joint torques of the Franka arm directly\n \n \n \n \n Controller for commanding joint velocity of the Franka arm using internal joint velocity controller\n \n \n\n" }, { "path": "franka_ros_controllers/include/franka_ros_controllers/effort_joint_impedance_controller.h", "content": "/***************************************************************************\n\n*\n* @package: franka_ros_controllers\n* @metapackage: franka_ros_interface\n* @author: Saif Sidhik \n*\n\n**************************************************************************/\n\n/***************************************************************************\n* Copyright (c) 2019-2021, Saif Sidhik.\n*\n* Licensed under the Apache License, Version 2.0 (the \"License\");\n* you may not use this file except in compliance with the License.\n* You may obtain a copy of the License at\n*\n* http://www.apache.org/licenses/LICENSE-2.0\n*\n* Unless required by applicable law or agreed to in writing, software\n* distributed under the License is distributed on an \"AS IS\" BASIS,\n* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n* See the License for the specific language governing permissions and\n* limitations under the License.\n**************************************************************************/\n\n#pragma once\n\n#include \n#include \n#include \n\n#include \n#include \n#include \n#include \n#include \n\n#include \n#include \n\n#include \n#include \n#include \n#include \n#include \n#include \n\n#include \n\n\nnamespace franka_ros_controllers {\n\nclass EffortJointImpedanceController : public controller_interface::MultiInterfaceController<\n franka_hw::FrankaModelInterface,\n franka_hw::FrankaStateInterface,\n hardware_interface::EffortJointInterface> {\n public:\n bool init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& node_handle) override;\n void starting(const ros::Time&) override;\n void update(const ros::Time&, const ros::Duration& period) override;\n\n private:\n // Saturation\n std::array saturateTorqueRate(\n const std::array& tau_d_calculated,\n const std::array& tau_J_d); // NOLINT (readability-identifier-naming)\n\n std::unique_ptr model_handle_;\n std::vector joint_handles_;\n\n static constexpr double kDeltaTauMax{1.0};\n\n double filter_params_{0.005};\n std::vector k_gains_;\n std::vector k_gains_target_;\n std::vector d_gains_;\n std::vector d_gains_target_;\n double coriolis_factor_{1.0};\n std::array pos_d_;\n std::array initial_pos_;\n std::array prev_pos_;\n std::array pos_d_target_;\n std::array dq_d_;\n std::array dq_filtered_;\n\n\n franka_hw::FrankaStateInterface* franka_state_interface_{};\n std::unique_ptr franka_state_handle_{};\n\n franka_hw::TriggerRate rate_trigger_{1.0};\n std::array last_tau_d_{};\n\n ros::Subscriber desired_joints_subscriber_;\n std::unique_ptr< dynamic_reconfigure::Server > dynamic_server_controller_config_;\n ros::NodeHandle dynamic_reconfigure_controller_gains_node_;\n\n franka_core_msgs::JointLimits joint_limits_;\n\n franka_hw::TriggerRate trigger_publish_;\n realtime_tools::RealtimePublisher publisher_controller_states_;\n\n bool checkPositionLimits(std::vector positions);\n bool checkVelocityLimits(std::vector positions);\n\n void controllerConfigCallback(franka_ros_controllers::joint_controller_paramsConfig& config,\n uint32_t level);\n void jointCmdCallback(const franka_core_msgs::JointCommandConstPtr& msg);\n};\n\n} // namespace franka_ros_controllers" }, { "path": "franka_ros_controllers/include/franka_ros_controllers/effort_joint_position_controller.h", "content": "/***************************************************************************\n\n*\n* @package: franka_ros_controllers\n* @metapackage: franka_ros_interface\n* @author: Saif Sidhik \n*\n\n**************************************************************************/\n\n/***************************************************************************\n* Copyright (c) 2019-2021, Saif Sidhik.\n*\n* Licensed under the Apache License, Version 2.0 (the \"License\");\n* you may not use this file except in compliance with the License.\n* You may obtain a copy of the License at\n*\n* http://www.apache.org/licenses/LICENSE-2.0\n*\n* Unless required by applicable law or agreed to in writing, software\n* distributed under the License is distributed on an \"AS IS\" BASIS,\n* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n* See the License for the specific language governing permissions and\n* limitations under the License.\n**************************************************************************/\n#pragma once\n\n#include \n#include \n#include \n\n#include \n#include \n#include \n#include \n#include \n\n#include \n#include \n\n#include \n#include \n#include \n#include \n#include \n#include \n#include \n\nnamespace franka_ros_controllers {\n\nclass EffortJointPositionController : public controller_interface::MultiInterfaceController<\n franka_hw::FrankaStateInterface,\n hardware_interface::EffortJointInterface> {\n public:\n bool init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& node_handle) override;\n void starting(const ros::Time&) override;\n void update(const ros::Time&, const ros::Duration& period) override;\n\n private:\n // Saturation\n std::array saturateTorqueRate(\n const std::array& tau_d_calculated,\n const std::array& tau_J_d); // NOLINT (readability-identifier-naming)\n\n std::vector joint_handles_;\n\n static constexpr double kDeltaTauMax{1.0};\n\n double filter_params_{0.005};\n std::vector k_gains_;\n std::vector k_gains_target_;\n std::vector d_gains_;\n std::vector d_gains_target_;\n double coriolis_factor_{1.0};\n std::array pos_d_;\n std::array initial_pos_;\n std::array prev_pos_;\n std::array pos_d_target_;\n std::array d_error_;\n std::array p_error_last_;\n \n franka_hw::FrankaStateInterface* franka_state_interface_{};\n std::unique_ptr franka_state_handle_{};\n\n franka_hw::TriggerRate rate_trigger_{1.0};\n \n ros::Subscriber desired_joints_subscriber_;\n std::unique_ptr< dynamic_reconfigure::Server > dynamic_server_controller_config_;\n ros::NodeHandle dynamic_reconfigure_controller_gains_node_;\n\n franka_core_msgs::JointLimits joint_limits_;\n\n franka_hw::TriggerRate trigger_publish_;\n realtime_tools::RealtimePublisher publisher_controller_states_;\n\n bool checkPositionLimits(std::vector positions);\n\n void controllerConfigCallback(franka_ros_controllers::joint_controller_paramsConfig& config,\n uint32_t level);\n void jointCmdCallback(const franka_core_msgs::JointCommandConstPtr& msg);\n};\n\n} // namespace franka_ros_controllers" }, { "path": "franka_ros_controllers/include/franka_ros_controllers/effort_joint_torque_controller.h", "content": "/***************************************************************************\n\n*\n* @package: franka_ros_controllers\n* @metapackage: franka_ros_interface\n* @author: Saif Sidhik \n*\n\n**************************************************************************/\n\n/***************************************************************************\n* Copyright (c) 2019-2021, Saif Sidhik.\n*\n* Licensed under the Apache License, Version 2.0 (the \"License\");\n* you may not use this file except in compliance with the License.\n* You may obtain a copy of the License at\n*\n* http://www.apache.org/licenses/LICENSE-2.0\n*\n* Unless required by applicable law or agreed to in writing, software\n* distributed under the License is distributed on an \"AS IS\" BASIS,\n* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n* See the License for the specific language governing permissions and\n* limitations under the License.\n**************************************************************************/\n#pragma once\n\n#include \n#include \n#include \n\n#include \n#include \n#include \n#include \n\n#include \n#include \n\n#include \n#include \n#include \n#include \n#include \n#include \n\n#include \n\nnamespace franka_ros_controllers {\n\nclass EffortJointTorqueController : public controller_interface::MultiInterfaceController<\n franka_hw::FrankaModelInterface,\n hardware_interface::EffortJointInterface> {\n public:\n bool init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& node_handle) override;\n void starting(const ros::Time&) override;\n void update(const ros::Time&, const ros::Duration& period) override;\n\n private:\n // Saturation\n static std::array saturateTorqueRate(\n const std::array& tau_d_calculated,\n const std::array& prev_tau); // NOLINT (readability-identifier-naming)\n\n std::unique_ptr model_handle_;\n std::vector joint_handles_;\n\n static constexpr double kDeltaTauMax{1.0};\n double coriolis_factor_{1.0};\n\n std::array jnt_cmd_{};\n std::array prev_jnt_cmd_{};\n\n\n franka_hw::TriggerRate rate_trigger_{1.0};\n std::array last_tau_d_{};\n\n ros::Subscriber desired_joints_subscriber_;\n\n franka_core_msgs::JointLimits joint_limits_;\n\n franka_hw::TriggerRate trigger_publish_;\n realtime_tools::RealtimePublisher publisher_controller_states_;\n\n bool checkTorqueLimits(std::vector torques);\n\n void jointCmdCallback(const franka_core_msgs::JointCommandConstPtr& msg);\n};\n\n} // namespace franka_ros_controllers" }, { "path": "franka_ros_controllers/include/franka_ros_controllers/position_joint_position_controller.h", "content": "/***************************************************************************\n\n*\n* @package: franka_ros_controllers\n* @metapackage: franka_ros_interface\n* @author: Saif Sidhik \n*\n\n**************************************************************************/\n\n/***************************************************************************\n* Copyright (c) 2019-2021, Saif Sidhik.\n*\n* Licensed under the Apache License, Version 2.0 (the \"License\");\n* you may not use this file except in compliance with the License.\n* You may obtain a copy of the License at\n*\n* http://www.apache.org/licenses/LICENSE-2.0\n*\n* Unless required by applicable law or agreed to in writing, software\n* distributed under the License is distributed on an \"AS IS\" BASIS,\n* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n* See the License for the specific language governing permissions and\n* limitations under the License.\n**************************************************************************/\n#pragma once\n\n#include \n#include \n#include \n\n#include \n#include \n\n#include \n#include \n#include \n\n#include \n#include \n#include \n\n#include \n\n#include \n#include \n#include \n#include \n\n\nnamespace franka_ros_controllers {\n\nclass PositionJointPositionController : public controller_interface::MultiInterfaceController<\n hardware_interface::PositionJointInterface> {\n public:\n bool init(hardware_interface::RobotHW* robot_hardware, ros::NodeHandle& node_handle) override;\n void starting(const ros::Time&) override;\n void update(const ros::Time&, const ros::Duration& period) override;\n\n private:\n hardware_interface::PositionJointInterface* position_joint_interface_;\n std::vector position_joint_handles_;\n std::array initial_pos_{};\n std::array prev_pos_{};\n std::array pos_d_target_{};\n std::array pos_d_;\n\n // joint_cmd subscriber\n ros::Subscriber desired_joints_subscriber_;\n\n double filter_joint_pos_{0.3};\n double target_filter_joint_pos_{0.3};\n double filter_factor_{0.01};\n\n double param_change_filter_{0.005};\n\n franka_core_msgs::JointLimits joint_limits_;\n \n // Dynamic reconfigure\n std::unique_ptr< dynamic_reconfigure::Server > dynamic_server_joint_controller_params_;\n ros::NodeHandle dynamic_reconfigure_joint_controller_params_node_;\n\n franka_hw::TriggerRate trigger_publish_;\n realtime_tools::RealtimePublisher publisher_controller_states_;\n\n bool checkPositionLimits(std::vector positions);\n\n\n void jointControllerParamCallback(franka_ros_controllers::joint_controller_paramsConfig& config,\n uint32_t level);\n void jointPosCmdCallback(const franka_core_msgs::JointCommandConstPtr& msg);\n};\n\n} // namespace franka_ros_controllers\n" }, { "path": "franka_ros_controllers/include/franka_ros_controllers/velocity_joint_velocity_controller.h", "content": "/***************************************************************************\n\n*\n* @package: franka_ros_controllers\n* @metapackage: franka_ros_interface\n* @author: Saif Sidhik \n*\n\n**************************************************************************/\n\n/***************************************************************************\n* Copyright (c) 2019-2021, Saif Sidhik.\n*\n* Licensed under the Apache License, Version 2.0 (the \"License\");\n* you may not use this file except in compliance with the License.\n* You may obtain a copy of the License at\n*\n* http://www.apache.org/licenses/LICENSE-2.0\n*\n* Unless required by applicable law or agreed to in writing, software\n* distributed under the License is distributed on an \"AS IS\" BASIS,\n* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n* See the License for the specific language governing permissions and\n* limitations under the License.\n**************************************************************************/\n#pragma once\n\n#include \n#include \n\n#include \n#include \n\n#include \n#include \n#include \n\n#include \n#include \n#include \n\n#include \n#include \n#include \n#include \n#include \n#include \n\nnamespace franka_ros_controllers {\n\nclass VelocityJointVelocityController : public controller_interface::MultiInterfaceController<\n hardware_interface::VelocityJointInterface,\n franka_hw::FrankaStateInterface> {\n public:\n bool init(hardware_interface::RobotHW* robot_hardware, ros::NodeHandle& node_handle) override;\n void update(const ros::Time&, const ros::Duration& period) override;\n void starting(const ros::Time&) override;\n void stopping(const ros::Time&) override;\n\n private:\n hardware_interface::VelocityJointInterface* velocity_joint_interface_;\n std::vector velocity_joint_handles_;\n std::array initial_vel_{};\n std::array prev_d_{};\n std::array vel_d_target_{};\n std::array vel_d_;\n\n // joint_cmd subscriber\n ros::Subscriber desired_joints_subscriber_;\n\n double filter_joint_vel_{0.3};\n double target_filter_joint_vel_{0.3};\n double filter_factor_{0.01};\n\n double param_change_filter_{0.005};\n\n franka_core_msgs::JointLimits joint_limits_;\n\n // Dynamic reconfigure\n std::unique_ptr< dynamic_reconfigure::Server > dynamic_server_joint_controller_params_;\n ros::NodeHandle dynamic_reconfigure_joint_controller_params_node_;\n\n franka_hw::TriggerRate trigger_publish_;\n realtime_tools::RealtimePublisher publisher_controller_states_;\n\n bool checkVelocityLimits(std::vector velocities);\n\n\n void jointControllerParamCallback(franka_ros_controllers::joint_controller_paramsConfig& config,\n uint32_t level);\n void jointVelCmdCallback(const franka_core_msgs::JointCommandConstPtr& msg);\n};\n\n} // namespace franka_ros_controllers\n" }, { "path": "franka_ros_controllers/package.xml", "content": "\n\n franka_ros_controllers\n 0.7.1\n test control\n Saif Sidhik\n Apache 2.0\n\n Saif Sidhik\n\n catkin\n\n message_generation\n eigen\n\n message_runtime\n\n controller_interface\n dynamic_reconfigure\n franka_hw\n geometry_msgs\n hardware_interface\n franka_core_msgs\n libfranka\n pluginlib\n realtime_tools\n roscpp\n\n franka_control\n franka_description\n message_runtime\n rospy\n\n \n \n \n\n" }, { "path": "franka_ros_controllers/scripts/test_controller.py", "content": "import rospy\nfrom franka_interface import ArmInterface\nimport numpy as np\nfrom builtins import input\n# import matplotlib.pyplot as plt\n# from std_msgs.msg import Float64\nfrom copy import deepcopy\n\nnames = ['panda_joint1','panda_joint2','panda_joint3','panda_joint4','panda_joint5','panda_joint6','panda_joint7']\n\nif __name__ == '__main__':\n \n\n rospy.init_node(\"test_node\")\n r = ArmInterface()\n\n rate = rospy.Rate(400)\n\n elapsed_time_ = rospy.Duration(0.0)\n period = rospy.Duration(0.005)\n\n r.move_to_neutral() # move to neutral pose before beginning\n\n initial_pose = deepcopy(r.joint_ordered_angles())\n\n input(\"Hit Enter to Start\")\n print(\"commanding\")\n vals = deepcopy(initial_pose)\n count = 0\n\n\n while not rospy.is_shutdown():\n\n elapsed_time_ += period\n\n delta = 3.14 / 16.0 * (1 - np.cos(3.14 / 5.0 * elapsed_time_.to_sec())) * 0.2\n\n for j, _ in enumerate(vals):\n if j == 4:\n vals[j] = initial_pose[j] - delta\n else:\n vals[j] = initial_pose[j] + delta\n\n if count%500 == 0:\n print(vals, delta)\n print(\"\\n ---- \\n\")\n print(\" \")\n\n\n # r.set_joint_positions_velocities(vals, [0.0 for _ in range(7)]) # for impedance control\n r.set_joint_positions(dict(zip(r.joint_names(), vals))) # try this for position control \n\n count += 1\n rate.sleep()" }, { "path": "franka_ros_controllers/src/effort_joint_impedance_controller.cpp", "content": "/***************************************************************************\n\n*\n* @package: franka_ros_controllers\n* @metapackage: franka_ros_interface\n* @author: Saif Sidhik \n*\n\n**************************************************************************/\n\n/***************************************************************************\n* Copyright (c) 2019-2021, Saif Sidhik.\n*\n* Licensed under the Apache License, Version 2.0 (the \"License\");\n* you may not use this file except in compliance with the License.\n* You may obtain a copy of the License at\n*\n* http://www.apache.org/licenses/LICENSE-2.0\n*\n* Unless required by applicable law or agreed to in writing, software\n* distributed under the License is distributed on an \"AS IS\" BASIS,\n* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n* See the License for the specific language governing permissions and\n* limitations under the License.\n**************************************************************************/\n#include \n\n#include \n#include \n\n#include \n#include \n#include \n\n#include \n\nnamespace franka_ros_controllers {\n\nbool EffortJointImpedanceController::init(hardware_interface::RobotHW* robot_hw,\n ros::NodeHandle& node_handle) {\n std::string arm_id;\n if (!node_handle.getParam(\"/robot_config/arm_id\", arm_id)) {\n ROS_ERROR(\"EffortJointImpedanceController: Could not read parameter arm_id\");\n return false;\n }\n\n franka_state_interface_ = robot_hw->get();\n if (franka_state_interface_ == nullptr) {\n ROS_ERROR(\"EffortJointImpedanceController: Could not get Franka state interface from hardware\");\n return false;\n }\n if (!node_handle.getParam(\"/robot_config/joint_names\", joint_limits_.joint_names) || joint_limits_.joint_names.size() != 7) {\n ROS_ERROR(\n \"EffortJointImpedanceController: Invalid or no joint_names parameters provided, aborting \"\n \"controller init!\");\n return false;\n }\n\n if (!node_handle.getParam(\"k_gains\", k_gains_) || k_gains_.size() != 7) {\n ROS_ERROR(\n \"EffortJointImpedanceController: Invalid or no k_gain parameters provided, aborting \"\n \"controller init!\");\n return false;\n }\n\n if (!node_handle.getParam(\"d_gains\", d_gains_) || d_gains_.size() != 7) {\n ROS_ERROR(\n \"EffortJointImpedanceController: Invalid or no d_gain parameters provided, aborting \"\n \"controller init!\");\n return false;\n }\n\n k_gains_target_.clear();\n d_gains_target_.clear();\n\n // k_gains_target_.resize(k_gains_.size());\n // d_gains_target_.resize(d_gains_.size());\n\n std::map pos_limit_lower_map;\n std::map pos_limit_upper_map;\n if (!node_handle.getParam(\"/robot_config/joint_config/joint_position_limit/lower\", pos_limit_lower_map) ) {\n ROS_ERROR(\n \"EffortJointImpedanceController: Joint limits parameters not provided, aborting \"\n \"controller init!\");\n return false;\n }\n if (!node_handle.getParam(\"/robot_config/joint_config/joint_position_limit/upper\", pos_limit_upper_map) ) {\n ROS_ERROR(\n \"EffortJointImpedanceController: Joint limits parameters not provided, aborting \"\n \"controller init!\");\n return false;\n }\n\n\n std::map velocity_limit_map;\n if (!node_handle.getParam(\"/robot_config/joint_config/joint_velocity_limit\", velocity_limit_map))\n {\n ROS_ERROR(\"EffortJointImpedanceController: Failed to find joint velocity limits on the param server. Aborting controller init\");\n return false;\n }\n\n for (size_t i = 0; i < joint_limits_.joint_names.size(); ++i){\n if (pos_limit_lower_map.find(joint_limits_.joint_names[i]) != pos_limit_lower_map.end())\n {\n joint_limits_.position_lower.push_back(pos_limit_lower_map[joint_limits_.joint_names[i]]);\n }\n else\n {\n ROS_ERROR(\"EffortJointImpedanceController: Unable to find lower position limit values for joint %s...\",\n joint_limits_.joint_names[i].c_str());\n }\n if (pos_limit_upper_map.find(joint_limits_.joint_names[i]) != pos_limit_upper_map.end())\n {\n joint_limits_.position_upper.push_back(pos_limit_upper_map[joint_limits_.joint_names[i]]);\n }\n else\n {\n ROS_ERROR(\"EffortJointImpedanceController: Unable to find upper position limit values for joint %s...\",\n joint_limits_.joint_names[i].c_str());\n }\n if (velocity_limit_map.find(joint_limits_.joint_names[i]) != velocity_limit_map.end())\n {\n joint_limits_.velocity.push_back(velocity_limit_map[joint_limits_.joint_names[i]]);\n }\n else\n {\n ROS_ERROR(\"EffortJointImpedanceController: Unable to find velocity limit values for joint %s...\",\n joint_limits_.joint_names[i].c_str());\n }\n // ROS_INFO_STREAM(\"K: \"<get();\n if (model_interface == nullptr) {\n ROS_ERROR_STREAM(\n \"EffortJointImpedanceController: Error getting model interface from hardware\");\n return false;\n }\n try {\n model_handle_ = std::make_unique(\n model_interface->getHandle(arm_id + \"_model\"));\n } catch (hardware_interface::HardwareInterfaceException& ex) {\n ROS_ERROR_STREAM(\n \"EffortJointImpedanceController: Exception getting model handle from interface: \"\n << ex.what());\n return false;\n }\n\n try {\n franka_state_handle_ = std::make_unique(\n franka_state_interface_->getHandle(arm_id + \"_robot\"));\n } catch (const hardware_interface::HardwareInterfaceException& ex) {\n ROS_ERROR_STREAM(\"EffortJointImpedanceController: Exception getting franka state handle: \" << ex.what());\n return false;\n }\n\n\n auto* effort_joint_interface = robot_hw->get();\n if (effort_joint_interface == nullptr) {\n ROS_ERROR_STREAM(\n \"EffortJointImpedanceController: Error getting effort joint interface from hardware\");\n return false;\n }\n for (size_t i = 0; i < 7; ++i) {\n try {\n joint_handles_.push_back(effort_joint_interface->getHandle(joint_limits_.joint_names[i]));\n } catch (const hardware_interface::HardwareInterfaceException& ex) {\n ROS_ERROR_STREAM(\n \"EffortJointImpedanceController: Exception getting joint handles: \" << ex.what());\n return false;\n }\n\n k_gains_target_.push_back(k_gains_[i]);\n d_gains_target_.push_back(d_gains_[i]);\n }\n\n\n dynamic_reconfigure_controller_gains_node_ =\n ros::NodeHandle(\"/franka_ros_interface/effort_joint_impedance_controller/arm/controller_parameters_config\");\n\n dynamic_server_controller_config_ = std::make_unique<\n dynamic_reconfigure::Server>(\n\n dynamic_reconfigure_controller_gains_node_);\n dynamic_server_controller_config_->setCallback(\n boost::bind(&EffortJointImpedanceController::controllerConfigCallback, this, _1, _2));\n\n desired_joints_subscriber_ = node_handle.subscribe(\n \"/franka_ros_interface/motion_controller/arm/joint_commands\", 20, &EffortJointImpedanceController::jointCmdCallback, this,\n ros::TransportHints().reliable().tcpNoDelay());\n\n publisher_controller_states_.init(node_handle, \"/franka_ros_interface/motion_controller/arm/joint_controller_states\", 1);\n\n {\n std::lock_guard > lock(\n publisher_controller_states_);\n publisher_controller_states_.msg_.controller_name = \"effort_joint_impedance_controller\";\n publisher_controller_states_.msg_.names.resize(joint_limits_.joint_names.size());\n publisher_controller_states_.msg_.joint_controller_states.resize(joint_limits_.joint_names.size());\n\n }\n\n std::fill(dq_filtered_.begin(), dq_filtered_.end(), 0);\n\n for (size_t i = 0; i < 7; ++i) { // this has to be done again; apparently when the dyn callback is initialised everything is set to zeros again!?\n k_gains_target_[i] = k_gains_[i];\n d_gains_target_[i] = d_gains_[i];\n }\n\n return true;\n}\n\nvoid EffortJointImpedanceController::starting(const ros::Time& /*time*/) {\n franka::RobotState robot_state = franka_state_handle_->getRobotState();\n\n for (size_t i = 0; i < 7; ++i) {\n initial_pos_[i] = robot_state.q[i];\n std::cout << \"---- Joint Number: \" << i << std::endl;\n std::cout << \"Joint Pos: \" << initial_pos_[i] << std::endl;\n std::cout << \"K_des: \" << k_gains_target_[i] << std::endl;\n std::cout << \"D_des: \" << d_gains_target_[i] << std::endl;\n std::cout << std::endl;\n }\n prev_pos_ = initial_pos_;\n pos_d_target_ = initial_pos_;\n\n std::fill(dq_filtered_.begin(), dq_filtered_.end(), 0);\n dq_d_ = dq_filtered_;\n}\n\nvoid EffortJointImpedanceController::update(const ros::Time& time,\n const ros::Duration& period) {\n franka::RobotState robot_state = franka_state_handle_->getRobotState();\n std::array coriolis = model_handle_->getCoriolis();\n\n double alpha = 0.99;\n for (size_t i = 0; i < 7; i++) {\n dq_filtered_[i] = (1 - alpha) * dq_filtered_[i] + alpha * robot_state.dq[i];\n }\n\n std::array tau_d_calculated{};\n for (size_t i = 0; i < 7; ++i) {\n tau_d_calculated[i] = coriolis_factor_ * coriolis[i] +\n k_gains_[i] * (pos_d_target_[i] - robot_state.q[i]) +\n d_gains_[i] * (dq_d_[i] - dq_filtered_[i]);\n }\n\n // Maximum torque difference with a sampling rate of 1 kHz. The maximum torque rate is\n // 1000 * (1 / sampling_time).\n std::array tau_d_saturated = saturateTorqueRate(tau_d_calculated, robot_state.tau_J_d);\n\n if (trigger_publish_() && publisher_controller_states_.trylock()) {\n for (size_t i = 0; i < 7; ++i){\n\n publisher_controller_states_.msg_.joint_controller_states[i].set_point = pos_d_target_[i];\n publisher_controller_states_.msg_.joint_controller_states[i].process_value = robot_state.q[i];\n publisher_controller_states_.msg_.joint_controller_states[i].process_value_dot = robot_state.dq[i];\n publisher_controller_states_.msg_.joint_controller_states[i].error = pos_d_target_[i] - robot_state.q[i];\n publisher_controller_states_.msg_.joint_controller_states[i].time_step = period.toSec();\n publisher_controller_states_.msg_.joint_controller_states[i].command = tau_d_calculated[i];\n\n publisher_controller_states_.msg_.joint_controller_states[i].p = k_gains_[i];\n publisher_controller_states_.msg_.joint_controller_states[i].d = d_gains_[i];\n publisher_controller_states_.msg_.joint_controller_states[i].header.stamp = time;\n }\n\n publisher_controller_states_.unlockAndPublish();\n }\n\n for (size_t i = 0; i < 7; ++i) {\n joint_handles_[i].setCommand(tau_d_saturated[i]);\n\n prev_pos_[i] = robot_state.q[i];\n\n k_gains_[i] = filter_params_ * k_gains_target_[i] + (1.0 - filter_params_) * k_gains_[i];\n d_gains_[i] = filter_params_ * d_gains_target_[i] + (1.0 - filter_params_) * d_gains_[i];\n\n\n }\n\n}\n\nbool EffortJointImpedanceController::checkPositionLimits(std::vector positions)\n{\n for (size_t i = 0; i < 7; ++i){\n if (!((positions[i] <= joint_limits_.position_upper[i]) && (positions[i] >= joint_limits_.position_lower[i]))){\n return true;\n }\n }\n\n return false;\n}\n\nbool EffortJointImpedanceController::checkVelocityLimits(std::vector velocities)\n{\n // bool retval = true;\n for (size_t i = 0; i < 7; ++i){\n if (!(abs(velocities[i]) <= joint_limits_.velocity[i])){\n return true;\n }\n }\n\n return false;\n}\n\nstd::array EffortJointImpedanceController::saturateTorqueRate(\n const std::array& tau_d_calculated,\n const std::array& tau_J_d) { // NOLINT (readability-identifier-naming)\n std::array tau_d_saturated{};\n for (size_t i = 0; i < 7; i++) {\n double difference = tau_d_calculated[i] - tau_J_d[i];\n tau_d_saturated[i] = tau_J_d[i] + std::max(std::min(difference, kDeltaTauMax), -kDeltaTauMax);\n }\n return tau_d_saturated;\n}\n\nvoid EffortJointImpedanceController::jointCmdCallback(const franka_core_msgs::JointCommandConstPtr& msg) {\n\n if (msg->mode == franka_core_msgs::JointCommand::IMPEDANCE_MODE){\n if (msg->position.size() != 7) {\n ROS_ERROR_STREAM(\n \"EffortJointImpedanceController: Published Commands are not of size 7\");\n pos_d_target_ = prev_pos_;\n }\n else if (checkPositionLimits(msg->position) || checkVelocityLimits(msg->velocity)) {\n ROS_ERROR_STREAM(\n \"EffortJointImpedanceController: Commanded positions or velicities are beyond allowed position limits.\");\n pos_d_target_ = prev_pos_;\n\n }\n else {\n std::copy_n(msg->position.begin(), 7, pos_d_target_.begin());\n std::copy_n(msg->velocity.begin(), 7, dq_d_.begin()); // if velocity is not there, the controller fails!!\n }\n }\n // else ROS_ERROR_STREAM(\"EffortJointImpedanceController: Published Command msg are not of JointCommand::IMPEDANCE_MODE! Dropping message\");\n}\n\nvoid EffortJointImpedanceController::controllerConfigCallback(\n franka_ros_controllers::joint_controller_paramsConfig& config,\n uint32_t /*level*/) {\n ROS_DEBUG_STREAM(\"EffortJointImpedanceController: Updating Config\");\n k_gains_target_[0] = config.groups.controller_gains.j1_k;\n k_gains_target_[1] = config.groups.controller_gains.j2_k;\n k_gains_target_[2] = config.groups.controller_gains.j3_k;\n k_gains_target_[3] = config.groups.controller_gains.j4_k;\n k_gains_target_[4] = config.groups.controller_gains.j5_k;\n k_gains_target_[5] = config.groups.controller_gains.j6_k;\n k_gains_target_[6] = config.groups.controller_gains.j7_k;\n\n d_gains_target_[0] = config.groups.controller_gains.j1_d;\n d_gains_target_[1] = config.groups.controller_gains.j2_d;\n d_gains_target_[2] = config.groups.controller_gains.j3_d;\n d_gains_target_[3] = config.groups.controller_gains.j4_d;\n d_gains_target_[4] = config.groups.controller_gains.j5_d;\n d_gains_target_[5] = config.groups.controller_gains.j6_d;\n d_gains_target_[6] = config.groups.controller_gains.j7_d;\n\n}\n\n} // namespace franka_ros_controllers\n\nPLUGINLIB_EXPORT_CLASS(franka_ros_controllers::EffortJointImpedanceController,\n controller_interface::ControllerBase)\n" }, { "path": "franka_ros_controllers/src/effort_joint_position_controller.cpp", "content": "/***************************************************************************\n\n*\n* @package: franka_ros_controllers\n* @metapackage: franka_ros_interface\n* @author: Saif Sidhik \n*\n\n**************************************************************************/\n\n/***************************************************************************\n* Copyright (c) 2019-2021, Saif Sidhik.\n*\n* Licensed under the Apache License, Version 2.0 (the \"License\");\n* you may not use this file except in compliance with the License.\n* You may obtain a copy of the License at\n*\n* http://www.apache.org/licenses/LICENSE-2.0\n*\n* Unless required by applicable law or agreed to in writing, software\n* distributed under the License is distributed on an \"AS IS\" BASIS,\n* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n* See the License for the specific language governing permissions and\n* limitations under the License.\n**************************************************************************/\n#include \n\n#include \n#include \n\n#include \n#include \n#include \n\n#include \n\nnamespace franka_ros_controllers {\n\nbool EffortJointPositionController::init(hardware_interface::RobotHW* robot_hw,\n ros::NodeHandle& node_handle) {\n std::string arm_id;\n if (!node_handle.getParam(\"/robot_config/arm_id\", arm_id)) {\n ROS_ERROR(\"EffortJointPositionController: Could not read parameter arm_id\");\n return false;\n }\n\n franka_state_interface_ = robot_hw->get();\n if (franka_state_interface_ == nullptr) {\n ROS_ERROR(\"EffortJointPositionController: Could not get Franka state interface from hardware\");\n return false;\n }\n if (!node_handle.getParam(\"/robot_config/joint_names\", joint_limits_.joint_names) || joint_limits_.joint_names.size() != 7) {\n ROS_ERROR(\n \"EffortJointPositionController: Invalid or no joint_names parameters provided, aborting \"\n \"controller init!\");\n return false;\n }\n\n if (!node_handle.getParam(\"k_gains\", k_gains_) || k_gains_.size() != 7) {\n ROS_ERROR(\n \"EffortJointPositionController: Invalid or no k_gain parameters provided, aborting \"\n \"controller init!\");\n return false;\n }\n\n if (!node_handle.getParam(\"d_gains\", d_gains_) || d_gains_.size() != 7) {\n ROS_ERROR(\n \"EffortJointPositionController: Invalid or no d_gain parameters provided, aborting \"\n \"controller init!\");\n return false;\n }\n\n k_gains_target_.clear();\n d_gains_target_.clear();\n\n\n std::map pos_limit_lower_map;\n std::map pos_limit_upper_map;\n if (!node_handle.getParam(\"/robot_config/joint_config/joint_position_limit/lower\", pos_limit_lower_map) ) {\n ROS_ERROR(\n \"EffortJointPositionController: Joint limits parameters not provided, aborting \"\n \"controller init!\");\n return false;\n }\n if (!node_handle.getParam(\"/robot_config/joint_config/joint_position_limit/upper\", pos_limit_upper_map) ) {\n ROS_ERROR(\n \"EffortJointPositionController: Joint limits parameters not provided, aborting \"\n \"controller init!\");\n return false;\n }\n\n for (size_t i = 0; i < joint_limits_.joint_names.size(); ++i){\n if (pos_limit_lower_map.find(joint_limits_.joint_names[i]) != pos_limit_lower_map.end())\n {\n joint_limits_.position_lower.push_back(pos_limit_lower_map[joint_limits_.joint_names[i]]);\n }\n else\n {\n ROS_ERROR(\"EffortJointPositionController: Unable to find lower position limit values for joint %s...\",\n joint_limits_.joint_names[i].c_str());\n }\n if (pos_limit_upper_map.find(joint_limits_.joint_names[i]) != pos_limit_upper_map.end())\n {\n joint_limits_.position_upper.push_back(pos_limit_upper_map[joint_limits_.joint_names[i]]);\n }\n else\n {\n ROS_ERROR(\"EffortJointPositionController: Unable to find upper position limit values for joint %s...\",\n joint_limits_.joint_names[i].c_str());\n }\n } \n\n double controller_state_publish_rate(30.0);\n if (!node_handle.getParam(\"controller_state_publish_rate\", controller_state_publish_rate)) {\n ROS_INFO_STREAM(\"EffortJointPositionController: Did not find controller_state_publish_rate. Using default \"\n << controller_state_publish_rate << \" [Hz].\");\n }\n trigger_publish_ = franka_hw::TriggerRate(controller_state_publish_rate);\n\n\n try {\n franka_state_handle_ = std::make_unique(\n franka_state_interface_->getHandle(arm_id + \"_robot\"));\n } catch (const hardware_interface::HardwareInterfaceException& ex) {\n ROS_ERROR_STREAM(\"EffortJointPositionController: Exception getting franka state handle: \" << ex.what());\n return false;\n }\n\n\n auto* effort_joint_interface = robot_hw->get();\n if (effort_joint_interface == nullptr) {\n ROS_ERROR_STREAM(\n \"EffortJointPositionController: Error getting effort joint interface from hardware\");\n return false;\n }\n for (size_t i = 0; i < 7; ++i) {\n try {\n joint_handles_.push_back(effort_joint_interface->getHandle(joint_limits_.joint_names[i]));\n } catch (const hardware_interface::HardwareInterfaceException& ex) {\n ROS_ERROR_STREAM(\n \"EffortJointPositionController: Exception getting joint handles: \" << ex.what());\n return false;\n }\n k_gains_target_.push_back(k_gains_[i]);\n d_gains_target_.push_back(d_gains_[i]);\n }\n\n dynamic_reconfigure_controller_gains_node_ =\n ros::NodeHandle(\"/franka_ros_interface/effort_joint_position_controller/arm/controller_parameters_config\");\n\n dynamic_server_controller_config_ = std::make_unique<\n dynamic_reconfigure::Server>(\n\n dynamic_reconfigure_controller_gains_node_);\n dynamic_server_controller_config_->setCallback(\n boost::bind(&EffortJointPositionController::controllerConfigCallback, this, _1, _2));\n\n desired_joints_subscriber_ = node_handle.subscribe(\n \"/franka_ros_interface/motion_controller/arm/joint_commands\", 20, &EffortJointPositionController::jointCmdCallback, this,\n ros::TransportHints().reliable().tcpNoDelay());\n\n publisher_controller_states_.init(node_handle, \"/franka_ros_interface/motion_controller/arm/joint_controller_states\", 1);\n\n {\n std::lock_guard > lock(\n publisher_controller_states_);\n publisher_controller_states_.msg_.controller_name = \"effort_joint_position_controller\";\n publisher_controller_states_.msg_.names.resize(joint_limits_.joint_names.size());\n publisher_controller_states_.msg_.joint_controller_states.resize(joint_limits_.joint_names.size());\n\n }\n\n for (size_t i = 0; i < 7; ++i) { // this has to be done again; apparently when the dyn callback is initialised everything is set to zeros again!?\n k_gains_target_[i] = k_gains_[i];\n d_gains_target_[i] = d_gains_[i];\n }\n\n return true;\n}\n\nvoid EffortJointPositionController::starting(const ros::Time& /*time*/) {\n franka::RobotState robot_state = franka_state_handle_->getRobotState();\n for (size_t i = 0; i < 7; ++i) {\n initial_pos_[i] = robot_state.q[i];\n }\n prev_pos_ = initial_pos_;\n pos_d_target_ = initial_pos_;\n\n std::fill(p_error_last_.begin(), p_error_last_.end(), 0);\n d_error_ = p_error_last_;\n}\n\nvoid EffortJointPositionController::update(const ros::Time& time,\n const ros::Duration& period) {\n franka::RobotState robot_state = franka_state_handle_->getRobotState();\n\n std::array error = p_error_last_;\n std::array error_dot = d_error_;\n\n for (size_t i = 0; i < 7; i++) {\n\n error[i] = (pos_d_target_[i] - robot_state.q[i]);\n\n if (period.toSec() > 0.0)\n {\n error_dot[i] = (error[i] - p_error_last_[i]) / period.toSec();\n }\n\n }\n\n d_error_ = error_dot;\n\n // Compute torque command using PD control law\n std::array tau_d_calculated{};\n for (size_t i = 0; i < 7; ++i) {\n\n tau_d_calculated[i] = k_gains_[i] * error[i] + \n d_gains_[i] * d_error_[i];\n\n }\n\n p_error_last_ = error;\n\n // Maximum torque difference with a sampling rate of 1 kHz. The maximum torque rate is\n // 1000 * (1 / sampling_time).\n std::array tau_d_saturated = saturateTorqueRate(tau_d_calculated, robot_state.tau_J_d);\n\n if (trigger_publish_() && publisher_controller_states_.trylock()) {\n for (size_t i = 0; i < 7; ++i){\n\n publisher_controller_states_.msg_.joint_controller_states[i].set_point = pos_d_target_[i];\n publisher_controller_states_.msg_.joint_controller_states[i].process_value = robot_state.q[i];\n publisher_controller_states_.msg_.joint_controller_states[i].process_value_dot = d_error_[i];\n publisher_controller_states_.msg_.joint_controller_states[i].error = error[i];\n publisher_controller_states_.msg_.joint_controller_states[i].time_step = period.toSec();\n publisher_controller_states_.msg_.joint_controller_states[i].command = tau_d_calculated[i];\n\n publisher_controller_states_.msg_.joint_controller_states[i].p = k_gains_[i];\n publisher_controller_states_.msg_.joint_controller_states[i].d = d_gains_[i];\n publisher_controller_states_.msg_.joint_controller_states[i].header.stamp = time;\n\n }\n\n publisher_controller_states_.unlockAndPublish();\n\n }\n\n for (size_t i = 0; i < 7; ++i) {\n joint_handles_[i].setCommand(tau_d_saturated[i]);\n\n prev_pos_[i] = robot_state.q[i];\n\n k_gains_[i] = filter_params_ * k_gains_target_[i] + (1.0 - filter_params_) * k_gains_[i];\n d_gains_[i] = filter_params_ * d_gains_target_[i] + (1.0 - filter_params_) * d_gains_[i];\n }\n\n}\n\nbool EffortJointPositionController::checkPositionLimits(std::vector positions)\n{\n for (size_t i = 0; i < 7; ++i){\n if (!((positions[i] <= joint_limits_.position_upper[i]) && (positions[i] >= joint_limits_.position_lower[i]))){\n return true;\n }\n }\n\n return false;\n}\n\nstd::array EffortJointPositionController::saturateTorqueRate(\n const std::array& tau_d_calculated,\n const std::array& tau_J_d) { // NOLINT (readability-identifier-naming)\n std::array tau_d_saturated{};\n for (size_t i = 0; i < 7; i++) {\n double difference = tau_d_calculated[i] - tau_J_d[i];\n tau_d_saturated[i] = tau_J_d[i] + std::max(std::min(difference, kDeltaTauMax), -kDeltaTauMax);\n }\n return tau_d_saturated;\n}\n\nvoid EffortJointPositionController::jointCmdCallback(const franka_core_msgs::JointCommandConstPtr& msg) {\n\n if (msg->mode == franka_core_msgs::JointCommand::POSITION_MODE){\n if (msg->position.size() != 7) {\n ROS_ERROR_STREAM(\n \"EffortJointPositionController: Published Commands are not of size 7\");\n pos_d_target_ = prev_pos_;\n std::fill(p_error_last_.begin(), p_error_last_.end(), 0);\n d_error_ = p_error_last_;\n }\n else if (checkPositionLimits(msg->position)) {\n ROS_ERROR_STREAM(\n \"PositionJointPositionController: Commanded positions are beyond allowed position limits.\");\n pos_d_target_ = prev_pos_;\n std::fill(p_error_last_.begin(), p_error_last_.end(), 0);\n d_error_ = p_error_last_;\n }\n else {\n std::copy_n(msg->position.begin(), 7, pos_d_target_.begin());\n\n }\n }\n // else ROS_ERROR_STREAM(\"EffortJointPositionController: Published Command msg are not of JointCommand::POSITION_MODE! Dropping message\");\n}\n\nvoid EffortJointPositionController::controllerConfigCallback(\n franka_ros_controllers::joint_controller_paramsConfig& config,\n uint32_t /*level*/) {\n\n k_gains_target_[0] = config.j1_k;\n k_gains_target_[1] = config.j2_k;\n k_gains_target_[2] = config.j3_k;\n k_gains_target_[3] = config.j4_k;\n k_gains_target_[4] = config.j5_k;\n k_gains_target_[5] = config.j6_k;\n k_gains_target_[6] = config.j7_k;\n\n d_gains_target_[0] = config.j1_d;\n d_gains_target_[1] = config.j2_d;\n d_gains_target_[2] = config.j3_d;\n d_gains_target_[3] = config.j4_d;\n d_gains_target_[4] = config.j5_d;\n d_gains_target_[5] = config.j6_d;\n d_gains_target_[6] = config.j7_d;\n\n}\n\n} // namespace franka_ros_controllers\n\nPLUGINLIB_EXPORT_CLASS(franka_ros_controllers::EffortJointPositionController,\n controller_interface::ControllerBase)\n" }, { "path": "franka_ros_controllers/src/effort_joint_torque_controller.cpp", "content": "/***************************************************************************\n\n*\n* @package: franka_ros_controllers\n* @metapackage: franka_ros_interface\n* @author: Saif Sidhik \n*\n\n**************************************************************************/\n\n/***************************************************************************\n* Copyright (c) 2019-2021, Saif Sidhik.\n*\n* Licensed under the Apache License, Version 2.0 (the \"License\");\n* you may not use this file except in compliance with the License.\n* You may obtain a copy of the License at\n*\n* http://www.apache.org/licenses/LICENSE-2.0\n*\n* Unless required by applicable law or agreed to in writing, software\n* distributed under the License is distributed on an \"AS IS\" BASIS,\n* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n* See the License for the specific language governing permissions and\n* limitations under the License.\n**************************************************************************/\n#include \n\n#include \n#include \n\n#include \n#include \n#include \n\nnamespace franka_ros_controllers {\n\nbool EffortJointTorqueController::init(hardware_interface::RobotHW* robot_hw,\n ros::NodeHandle& node_handle) {\n std::string arm_id;\n if (!node_handle.getParam(\"/robot_config/arm_id\", arm_id)) {\n ROS_ERROR(\"EffortJointTorqueController: Could not read parameter arm_id\");\n return false;\n }\n if (!node_handle.getParam(\"/robot_config/joint_names\", joint_limits_.joint_names) || joint_limits_.joint_names.size() != 7) {\n ROS_ERROR(\n \"EffortJointTorqueController: Invalid or no joint_names parameters provided, aborting \"\n \"controller init!\");\n return false;\n }\n\n bool enable_coriolis;\n if (!node_handle.getParam(\"/franka_ros_interface/effort_joint_torque_controller/compensate_coriolis\", enable_coriolis)) {\n ROS_ERROR(\"EffortJointTorqueController: Could not read parameter compensate_coriolis\");\n return false;\n }\n\n if (enable_coriolis==false){\n coriolis_factor_ = 0.0;\n ROS_INFO_STREAM(\"EffortJointTorqueController: Coriolis compensation disabled!\");\n }\n else{\n ROS_INFO_STREAM(\"EffortJointTorqueController: Coriolis compensation enabled!\");\n }\n\n std::map torque_limit_map;\n if (!node_handle.getParam(\"/robot_config/joint_config/joint_effort_limit\", torque_limit_map))\n {\n ROS_ERROR(\"EffortJointTorqueController: Failed to find joint effort limits on the param server. Aborting controller init\");\n return false;\n }\n\n for (size_t i = 0; i < joint_limits_.joint_names.size(); ++i){\n if (torque_limit_map.find(joint_limits_.joint_names[i]) != torque_limit_map.end())\n {\n joint_limits_.effort.push_back(torque_limit_map[joint_limits_.joint_names[i]]);\n }\n else\n {\n ROS_ERROR(\"EffortJointTorqueController: Unable to find torque limit values for joint %s...\",\n joint_limits_.joint_names[i].c_str());\n }\n } \n\n double controller_state_publish_rate(30.0);\n if (!node_handle.getParam(\"controller_state_publish_rate\", controller_state_publish_rate)) {\n ROS_INFO_STREAM(\"EffortJointTorqueController: Did not find controller_state_publish_rate. Using default \"\n << controller_state_publish_rate << \" [Hz].\");\n }\n trigger_publish_ = franka_hw::TriggerRate(controller_state_publish_rate);\n\n auto* model_interface = robot_hw->get();\n if (model_interface == nullptr) {\n ROS_ERROR_STREAM(\n \"EffortJointTorqueController: Error getting model interface from hardware\");\n return false;\n }\n try {\n model_handle_ = std::make_unique(\n model_interface->getHandle(arm_id + \"_model\"));\n } catch (hardware_interface::HardwareInterfaceException& ex) {\n ROS_ERROR_STREAM(\n \"EffortJointTorqueController: Exception getting model handle from interface: \"\n << ex.what());\n return false;\n }\n\n auto* effort_joint_interface = robot_hw->get();\n if (effort_joint_interface == nullptr) {\n ROS_ERROR_STREAM(\n \"EffortJointTorqueController: Error getting effort joint interface from hardware\");\n return false;\n }\n for (size_t i = 0; i < 7; ++i) {\n try {\n joint_handles_.push_back(effort_joint_interface->getHandle(joint_limits_.joint_names[i]));\n } catch (const hardware_interface::HardwareInterfaceException& ex) {\n ROS_ERROR_STREAM(\n \"EffortJointTorqueController: Exception getting joint handles: \" << ex.what());\n return false;\n }\n }\n desired_joints_subscriber_ = node_handle.subscribe(\n \"/franka_ros_interface/motion_controller/arm/joint_commands\", 20, &EffortJointTorqueController::jointCmdCallback, this,\n ros::TransportHints().reliable().tcpNoDelay());\n publisher_controller_states_.init(node_handle, \"/franka_ros_interface/motion_controller/arm/joint_controller_states\", 1);\n\n {\n std::lock_guard > lock(\n publisher_controller_states_);\n publisher_controller_states_.msg_.controller_name = \"effort_joint_torque_controller\";\n publisher_controller_states_.msg_.names.resize(joint_limits_.joint_names.size());\n publisher_controller_states_.msg_.joint_controller_states.resize(joint_limits_.joint_names.size());\n\n }\n\n return true;\n}\n\nvoid EffortJointTorqueController::starting(const ros::Time& /*time*/) {\n\n std::fill(jnt_cmd_.begin(), jnt_cmd_.end(), 0);\n prev_jnt_cmd_ = jnt_cmd_;\n ROS_WARN_STREAM(\"EffortJointTorqueController: Using raw torque controller! Be extremely careful and send smooth commands.\");\n}\n\nvoid EffortJointTorqueController::update(const ros::Time& time,\n const ros::Duration& period) {\n \n std::array coriolis = model_handle_->getCoriolis();\n\n std::array compensated_cmd{};\n for (size_t i = 0; i < 7; ++i) {\n compensated_cmd[i] = coriolis_factor_ * coriolis[i] + jnt_cmd_[i];\n }\n // Maximum torque difference with a sampling rate of 1 kHz. The maximum torque rate is\n // 1000 * (1 / sampling_time).\n std::array tau_d_saturated = saturateTorqueRate(compensated_cmd, prev_jnt_cmd_);\n\n if (trigger_publish_() && publisher_controller_states_.trylock()) {\n for (size_t i = 0; i < 7; ++i){\n\n publisher_controller_states_.msg_.joint_controller_states[i].set_point = jnt_cmd_[i];\n publisher_controller_states_.msg_.joint_controller_states[i].process_value = compensated_cmd[i];\n publisher_controller_states_.msg_.joint_controller_states[i].time_step = period.toSec();\n publisher_controller_states_.msg_.joint_controller_states[i].command = tau_d_saturated[i];\n\n publisher_controller_states_.msg_.joint_controller_states[i].header.stamp = time;\n\n }\n\n publisher_controller_states_.unlockAndPublish();\n\n }\n\n for (size_t i = 0; i < 7; ++i) {\n joint_handles_[i].setCommand(tau_d_saturated[i]);\n\n prev_jnt_cmd_[i] = tau_d_saturated[i];\n\n }\n\n}\n\nbool EffortJointTorqueController::checkTorqueLimits(std::vector torques)\n{\n for (size_t i = 0; i < 7; ++i){\n if (!(abs(torques[i]) < joint_limits_.effort[i])){\n return true;\n }\n }\n\n return false;\n}\n\nstd::array EffortJointTorqueController::saturateTorqueRate(\n const std::array& tau_d_calculated, const std::array& prev_tau) { // NOLINT (readability-identifier-naming)\n std::array tau_d_saturated{};\n for (size_t i = 0; i < 7; i++) {\n double difference = tau_d_calculated[i] - prev_tau[i];\n tau_d_saturated[i] = prev_tau[i] + std::max(std::min(difference, kDeltaTauMax), -kDeltaTauMax);\n }\n return tau_d_saturated;\n}\n\nvoid EffortJointTorqueController::jointCmdCallback(const franka_core_msgs::JointCommandConstPtr& msg) {\n\n if (msg->mode == franka_core_msgs::JointCommand::TORQUE_MODE){\n if (msg->effort.size() != 7) {\n ROS_ERROR_STREAM(\n \"EffortJointTorqueController: Published Commands are not of size 7\");\n std::fill(jnt_cmd_.begin(), jnt_cmd_.end(), 0);\n jnt_cmd_= prev_jnt_cmd_;\n }\n else if (checkTorqueLimits(msg->effort)) {\n ROS_ERROR_STREAM(\n \"EffortJointTorqueController: Commanded torques are beyond allowed torque limits.\");\n std::fill(jnt_cmd_.begin(), jnt_cmd_.end(), 0);\n jnt_cmd_= prev_jnt_cmd_;\n }\n else {\n std::copy_n(msg->effort.begin(), 7, jnt_cmd_.begin());\n\n }\n }\n // else ROS_ERROR_STREAM(\"EffortJointTorqueController: Published Command msg are not of JointCommand::TORQUE_MODE! Dropping message\");\n}\n\n} // namespace franka_ros_controllers\n\nPLUGINLIB_EXPORT_CLASS(franka_ros_controllers::EffortJointTorqueController,\n controller_interface::ControllerBase)\n" }, { "path": "franka_ros_controllers/src/position_joint_position_controller.cpp", "content": "/***************************************************************************\n\n*\n* @package: franka_ros_controllers\n* @metapackage: franka_ros_interface\n* @author: Saif Sidhik \n*\n\n**************************************************************************/\n\n/***************************************************************************\n* Copyright (c) 2019-2021, Saif Sidhik.\n*\n* Licensed under the Apache License, Version 2.0 (the \"License\");\n* you may not use this file except in compliance with the License.\n* You may obtain a copy of the License at\n*\n* http://www.apache.org/licenses/LICENSE-2.0\n*\n* Unless required by applicable law or agreed to in writing, software\n* distributed under the License is distributed on an \"AS IS\" BASIS,\n* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n* See the License for the specific language governing permissions and\n* limitations under the License.\n**************************************************************************/\n#include \n\n#include \n\n#include \n#include \n#include \n#include \n#include \n\n\nnamespace franka_ros_controllers {\n\nbool PositionJointPositionController::init(hardware_interface::RobotHW* robot_hardware,\n ros::NodeHandle& node_handle) {\n\n desired_joints_subscriber_ = node_handle.subscribe(\n \"/franka_ros_interface/motion_controller/arm/joint_commands\", 20, &PositionJointPositionController::jointPosCmdCallback, this,\n ros::TransportHints().reliable().tcpNoDelay());\n\n position_joint_interface_ = robot_hardware->get();\n if (position_joint_interface_ == nullptr) {\n ROS_ERROR(\n \"PositionJointPositionController: Error getting position joint interface from hardware!\");\n return false;\n }\n if (!node_handle.getParam(\"/robot_config/joint_names\", joint_limits_.joint_names)) {\n ROS_ERROR(\"PositionJointPositionController: Could not parse joint names\");\n }\n if (joint_limits_.joint_names.size() != 7) {\n ROS_ERROR_STREAM(\"PositionJointPositionController: Wrong number of joint names, got \"\n << joint_limits_.joint_names.size() << \" instead of 7 names!\");\n return false;\n }\n std::map pos_limit_lower_map;\n std::map pos_limit_upper_map;\n if (!node_handle.getParam(\"/robot_config/joint_config/joint_position_limit/lower\", pos_limit_lower_map) ) {\n ROS_ERROR(\n \"PositionJointPositionController: Joint limits parameters not provided, aborting \"\n \"controller init!\");\n return false;\n }\n if (!node_handle.getParam(\"/robot_config/joint_config/joint_position_limit/upper\", pos_limit_upper_map) ) {\n ROS_ERROR(\n \"PositionJointPositionController: Joint limits parameters not provided, aborting \"\n \"controller init!\");\n return false;\n }\n\n for (size_t i = 0; i < joint_limits_.joint_names.size(); ++i){\n if (pos_limit_lower_map.find(joint_limits_.joint_names[i]) != pos_limit_lower_map.end())\n {\n joint_limits_.position_lower.push_back(pos_limit_lower_map[joint_limits_.joint_names[i]]);\n }\n else\n {\n ROS_ERROR(\"PositionJointPositionController: Unable to find lower position limit values for joint %s...\",\n joint_limits_.joint_names[i].c_str());\n }\n if (pos_limit_upper_map.find(joint_limits_.joint_names[i]) != pos_limit_upper_map.end())\n {\n joint_limits_.position_upper.push_back(pos_limit_upper_map[joint_limits_.joint_names[i]]);\n }\n else\n {\n ROS_ERROR(\"PositionJointPositionController: Unable to find upper position limit values for joint %s...\",\n joint_limits_.joint_names[i].c_str());\n }\n } \n\n position_joint_handles_.resize(7);\n for (size_t i = 0; i < 7; ++i) {\n try {\n position_joint_handles_[i] = position_joint_interface_->getHandle(joint_limits_.joint_names[i]);\n } catch (const hardware_interface::HardwareInterfaceException& e) {\n ROS_ERROR_STREAM(\n \"PositionJointPositionController: Exception getting joint handles: \" << e.what());\n return false;\n }\n }\n\n double controller_state_publish_rate(30.0);\n if (!node_handle.getParam(\"controller_state_publish_rate\", controller_state_publish_rate)) {\n ROS_INFO_STREAM(\"PositionJointPositionController: Did not find controller_state_publish_rate. Using default \"\n << controller_state_publish_rate << \" [Hz].\");\n }\n trigger_publish_ = franka_hw::TriggerRate(controller_state_publish_rate);\n\n dynamic_reconfigure_joint_controller_params_node_ =\n ros::NodeHandle(\"/franka_ros_interface/position_joint_position_controller/arm/controller_parameters_config\");\n\n dynamic_server_joint_controller_params_ = std::make_unique<\n dynamic_reconfigure::Server>(\n dynamic_reconfigure_joint_controller_params_node_);\n\n dynamic_server_joint_controller_params_->setCallback(\n boost::bind(&PositionJointPositionController::jointControllerParamCallback, this, _1, _2));\n\n publisher_controller_states_.init(node_handle, \"/franka_ros_interface/motion_controller/arm/joint_controller_states\", 1);\n\n {\n std::lock_guard > lock(\n publisher_controller_states_);\n publisher_controller_states_.msg_.controller_name = \"position_joint_position_controller\";\n publisher_controller_states_.msg_.names.resize(joint_limits_.joint_names.size());\n publisher_controller_states_.msg_.joint_controller_states.resize(joint_limits_.joint_names.size());\n\n }\n\n return true;\n}\n\nvoid PositionJointPositionController::starting(const ros::Time& /* time */) {\n for (size_t i = 0; i < 7; ++i) {\n initial_pos_[i] = position_joint_handles_[i].getPosition();\n }\n pos_d_ = initial_pos_;\n prev_pos_ = initial_pos_;\n pos_d_target_ = initial_pos_;\n}\n\nvoid PositionJointPositionController::update(const ros::Time& time,\n const ros::Duration& period) {\n for (size_t i = 0; i < 7; ++i) {\n position_joint_handles_[i].setCommand(pos_d_[i]);\n }\n double filter_val = filter_joint_pos_ * filter_factor_;\n for (size_t i = 0; i < 7; ++i) {\n prev_pos_[i] = position_joint_handles_[i].getPosition();\n pos_d_[i] = filter_val * pos_d_target_[i] + (1.0 - filter_val) * pos_d_[i];\n }\n\n if (trigger_publish_() && publisher_controller_states_.trylock()) {\n for (size_t i = 0; i < 7; ++i){\n\n publisher_controller_states_.msg_.joint_controller_states[i].set_point = pos_d_target_[i];\n publisher_controller_states_.msg_.joint_controller_states[i].process_value = pos_d_[i];\n publisher_controller_states_.msg_.joint_controller_states[i].time_step = period.toSec();\n\n publisher_controller_states_.msg_.joint_controller_states[i].header.stamp = time;\n\n }\n\n publisher_controller_states_.unlockAndPublish(); \n }\n\n // update parameters changed online either through dynamic reconfigure or through the interactive\n // target by filtering\n filter_joint_pos_ = param_change_filter_ * target_filter_joint_pos_ + (1.0 - param_change_filter_) * filter_joint_pos_;\n\n}\n\nbool PositionJointPositionController::checkPositionLimits(std::vector positions)\n{\n // bool retval = true;\n for (size_t i = 0; i < 7; ++i){\n if (!((positions[i] <= joint_limits_.position_upper[i]) && (positions[i] >= joint_limits_.position_lower[i]))){\n return true;\n }\n }\n\n return false;\n}\n\nvoid PositionJointPositionController::jointPosCmdCallback(const franka_core_msgs::JointCommandConstPtr& msg) {\n\n if (msg->mode == franka_core_msgs::JointCommand::POSITION_MODE){\n if (msg->position.size() != 7) {\n ROS_ERROR_STREAM(\n \"PositionJointPositionController: Published Commands are not of size 7\");\n pos_d_ = prev_pos_;\n pos_d_target_ = prev_pos_;\n }\n else if (checkPositionLimits(msg->position)) {\n ROS_ERROR_STREAM(\n \"PositionJointPositionController: Commanded positions are beyond allowed position limits.\");\n pos_d_ = prev_pos_;\n pos_d_target_ = prev_pos_;\n\n }\n else\n {\n std::copy_n(msg->position.begin(), 7, pos_d_target_.begin());\n }\n \n }\n // else ROS_ERROR_STREAM(\"PositionJointPositionController: Published Command msg are not of JointCommand::POSITION_MODE! Dropping message\");\n}\n\nvoid PositionJointPositionController::jointControllerParamCallback(franka_ros_controllers::joint_controller_paramsConfig& config,\n uint32_t level){\n target_filter_joint_pos_ = config.position_joint_delta_filter;\n}\n\n} // namespace franka_ros_controllers\n\nPLUGINLIB_EXPORT_CLASS(franka_ros_controllers::PositionJointPositionController,\n controller_interface::ControllerBase)\n" }, { "path": "franka_ros_controllers/src/velocity_joint_velocity_controller.cpp", "content": "/***************************************************************************\n\n*\n* @package: franka_ros_controllers\n* @metapackage: franka_ros_interface\n* @author: Saif Sidhik \n*\n\n**************************************************************************/\n\n/***************************************************************************\n* Copyright (c) 2019-2021, Saif Sidhik.\n*\n* Licensed under the Apache License, Version 2.0 (the \"License\");\n* you may not use this file except in compliance with the License.\n* You may obtain a copy of the License at\n*\n* http://www.apache.org/licenses/LICENSE-2.0\n*\n* Unless required by applicable law or agreed to in writing, software\n* distributed under the License is distributed on an \"AS IS\" BASIS,\n* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n* See the License for the specific language governing permissions and\n* limitations under the License.\n**************************************************************************/\n#include \n\n#include \n\n#include \n#include \n#include \n#include \n#include \n\n\nnamespace franka_ros_controllers {\n\nbool VelocityJointVelocityController::init(hardware_interface::RobotHW* robot_hardware,\n ros::NodeHandle& node_handle) {\n\n desired_joints_subscriber_ = node_handle.subscribe(\n \"/franka_ros_interface/motion_controller/arm/joint_commands\", 20, &VelocityJointVelocityController::jointVelCmdCallback, this,\n ros::TransportHints().reliable().tcpNoDelay());\n\n velocity_joint_interface_ = robot_hardware->get();\n if (velocity_joint_interface_ == nullptr) {\n ROS_ERROR(\n \"VelocityJointVelocityController: Error getting velocity joint interface from hardware!\");\n return false;\n }\n if (!node_handle.getParam(\"/robot_config/joint_names\", joint_limits_.joint_names)) {\n ROS_ERROR(\"VelocityJointVelocityController: Could not parse joint names\");\n }\n if (joint_limits_.joint_names.size() != 7) {\n ROS_ERROR_STREAM(\"VelocityJointVelocityController: Wrong number of joint names, got \"\n << joint_limits_.joint_names.size() << \" instead of 7 names!\");\n return false;\n }\n std::map vel_limit_map;\n if (!node_handle.getParam(\"/robot_config/joint_config/joint_velocity_limit\", vel_limit_map) ) {\n ROS_ERROR(\n \"VelocityJointVelocityController: Joint limits parameters not provided, aborting \"\n \"controller init!\");\n return false;\n }\n \n\n for (size_t i = 0; i < joint_limits_.joint_names.size(); ++i){\n if (vel_limit_map.find(joint_limits_.joint_names[i]) != vel_limit_map.end())\n {\n joint_limits_.velocity.push_back(vel_limit_map[joint_limits_.joint_names[i]]);\n }\n else\n {\n ROS_ERROR(\"VelocityJointVelocityController: Unable to find lower velocity limit values for joint %s...\",\n joint_limits_.joint_names[i].c_str());\n }\n } \n\n velocity_joint_handles_.resize(7);\n for (size_t i = 0; i < 7; ++i) {\n try {\n velocity_joint_handles_[i] = velocity_joint_interface_->getHandle(joint_limits_.joint_names[i]);\n } catch (const hardware_interface::HardwareInterfaceException& e) {\n ROS_ERROR_STREAM(\n \"VelocityJointVelocityController: Exception getting joint handles: \" << e.what());\n return false;\n }\n }\n\n double controller_state_publish_rate(30.0);\n if (!node_handle.getParam(\"controller_state_publish_rate\", controller_state_publish_rate)) {\n ROS_INFO_STREAM(\"VelocityJointVelocityController: Did not find controller_state_publish_rate. Using default \"\n << controller_state_publish_rate << \" [Hz].\");\n }\n trigger_publish_ = franka_hw::TriggerRate(controller_state_publish_rate);\n\n dynamic_reconfigure_joint_controller_params_node_ =\n ros::NodeHandle(\"/franka_ros_interface/velocity_joint_velocity_controller/arm/controller_parameters_config\");\n\n dynamic_server_joint_controller_params_ = std::make_unique<\n dynamic_reconfigure::Server>(\n dynamic_reconfigure_joint_controller_params_node_);\n\n dynamic_server_joint_controller_params_->setCallback(\n boost::bind(&VelocityJointVelocityController::jointControllerParamCallback, this, _1, _2));\n\n publisher_controller_states_.init(node_handle, \"/franka_ros_interface/motion_controller/arm/joint_controller_states\", 1);\n\n {\n std::lock_guard > lock(\n publisher_controller_states_);\n publisher_controller_states_.msg_.controller_name = \"velocity_joint_velocity_controller\";\n publisher_controller_states_.msg_.names.resize(joint_limits_.joint_names.size());\n publisher_controller_states_.msg_.joint_controller_states.resize(joint_limits_.joint_names.size());\n\n }\n\n return true;\n}\n\nvoid VelocityJointVelocityController::starting(const ros::Time& /* time */) {\n for (size_t i = 0; i < 7; ++i) {\n initial_vel_[i] = velocity_joint_handles_[i].getVelocity();\n }\n vel_d_ = initial_vel_;\n prev_d_ = vel_d_;\n}\n\nvoid VelocityJointVelocityController::update(const ros::Time& time,\n const ros::Duration& period) {\n for (size_t i = 0; i < 7; ++i) {\n velocity_joint_handles_[i].setCommand(vel_d_[i]);\n }\n double filter_val = filter_joint_vel_ * filter_factor_;\n for (size_t i = 0; i < 7; ++i) {\n prev_d_[i] = velocity_joint_handles_[i].getVelocity();\n vel_d_[i] = filter_val * vel_d_target_[i] + (1.0 - filter_val) * vel_d_[i];\n }\n\n if (trigger_publish_() && publisher_controller_states_.trylock()) {\n for (size_t i = 0; i < 7; ++i){\n\n publisher_controller_states_.msg_.joint_controller_states[i].set_point = vel_d_target_[i];\n publisher_controller_states_.msg_.joint_controller_states[i].process_value = vel_d_[i];\n publisher_controller_states_.msg_.joint_controller_states[i].time_step = period.toSec();\n\n publisher_controller_states_.msg_.joint_controller_states[i].header.stamp = time;\n\n }\n\n publisher_controller_states_.unlockAndPublish(); \n }\n\n // update parameters changed online either through dynamic reconfigure or through the interactive\n // target by filtering\n filter_joint_vel_ = param_change_filter_ * target_filter_joint_vel_ + (1.0 - param_change_filter_) * filter_joint_vel_;\n\n}\n\nbool VelocityJointVelocityController::checkVelocityLimits(std::vector velocities)\n{\n // bool retval = true;\n for (size_t i = 0; i < 7; ++i){\n if (!(abs(velocities[i]) <= joint_limits_.velocity[i])){\n return true;\n }\n }\n\n return false;\n}\n\nvoid VelocityJointVelocityController::jointVelCmdCallback(const franka_core_msgs::JointCommandConstPtr& msg) {\n\n if (msg->mode == franka_core_msgs::JointCommand::VELOCITY_MODE){\n if (msg->velocity.size() != 7) {\n ROS_ERROR_STREAM(\n \"VelocityJointVelocityController: Published Commands are not of size 7\");\n vel_d_ = prev_d_;\n vel_d_target_ = prev_d_;\n }\n else if (checkVelocityLimits(msg->velocity)) {\n ROS_ERROR_STREAM(\n \"VelocityJointVelocityController: Commanded velocities are beyond allowed velocity limits.\");\n vel_d_ = prev_d_;\n vel_d_target_ = prev_d_;\n\n }\n else\n {\n std::copy_n(msg->velocity.begin(), 7, vel_d_target_.begin());\n }\n \n }\n // else ROS_ERROR_STREAM(\"VelocityJointVelocityController: Published Command msg are not of JointCommand::Velocity! Dropping message\");\n}\n\nvoid VelocityJointVelocityController::jointControllerParamCallback(franka_ros_controllers::joint_controller_paramsConfig& config,\n uint32_t level){\n target_filter_joint_vel_ = config.velocity_joint_delta_filter;\n}\n\nvoid VelocityJointVelocityController::stopping(const ros::Time& /*time*/) {\n // WARNING: DO NOT SEND ZERO VELOCITIES HERE AS IN CASE OF ABORTING DURING MOTION\n // A JUMP TO ZERO WILL BE COMMANDED PUTTING HIGH LOADS ON THE ROBOT. LET THE DEFAULT\n // BUILT-IN STOPPING BEHAVIOR SLOW DOWN THE ROBOT.\n}\n\n} // namespace franka_ros_controllers\n\nPLUGINLIB_EXPORT_CLASS(franka_ros_controllers::VelocityJointVelocityController,\n controller_interface::ControllerBase)\n" }, { "path": "franka_ros_interface/CMakeLists.txt", "content": "cmake_minimum_required(VERSION 2.8.3)\nproject(franka_ros_interface)\nfind_package(catkin REQUIRED)\ncatkin_metapackage()\n" }, { "path": "franka_ros_interface/package.xml", "content": "\n\n franka_ros_interface\n 0.7.1\n \n franka_ros_interface metapackage containing all of the SDK packages\n for Franka Panda robots using libfranka and franka-ros.\n \n\n \n Saif Sidhik\n \n Apache 2.0\n Saif Sidhik\n\n catkin\n franka_interface\n franka_moveit\n franka_msgs\n franka_ros_controllers\n franka_tools\n \n \n \n \n \n\n\n" }, { "path": "franka_tools/CMakeLists.txt", "content": "cmake_minimum_required(VERSION 2.8.3)\nproject(franka_tools)\n\n## Find catkin macros and libraries\n## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)\n## is used, also find other catkin packages\nfind_package(catkin REQUIRED COMPONENTS\n roscpp\n rospy\n std_msgs\n franka_control\n franka_interface\n franka_core_msgs\n)\n\n## System dependencies are found with CMake's conventions\n# find_package(Boost REQUIRED COMPONENTS system)\n\n\ncatkin_python_setup()\n\n\n\n# include_directories(include)\ninclude_directories(\n ${catkin_INCLUDE_DIRS}\n)\n\n\n" }, { "path": "franka_tools/README.md", "content": "# franka_tools\n\nSome helper classes for controlling and handling the Franka Emika Panda robot.\n\n## FrankaControllerManagerInterface\n\n- List, start, stop, load available controllers for the robot\n- Get the current controller status (commands, set points, controller gains, etc.)\n- Update controller parameters through *ControllerParamConfigClient* (see below)\n\n## FrankaFramesInterface\n\n- Get and Set end-effector frame and stiffness frame of the robot easily\n- Set the frames to known frames (such as links on the robot) directly\n\n## ControllerParamConfigClient\n\n- Get and set the controller parameters (gains) for the active controller\n\n## JointTrajectoryActionClient\n\n- Command robot to given joint position(s) smoothly. (Uses the FollowJointTrajectory service from ROS *control_msgs* package)\n- Smoothly move to a desired (valid) pose without having to interpolate for smoothness (trajectory interpolation done internally)\n\n## CollisionBehaviourInterface\n\n- Define collision and contact thresholds for the robot safety and contact detection." }, { "path": "franka_tools/package.xml", "content": "\n\n franka_tools\n 0.7.1\n The franka_tools package\n\n \n \n \n Saif Sidhik\n\n Apache 2.0\n\n Saif Sidhik\n \n \n catkin\n roscpp\n rospy\n std_msgs\n franka_interface\n franka_core_msgs\n franka_control\n\n roscpp\n rospy\n std_msgs\n franka_interface\n franka_core_msgs\n franka_control\n\n \n \n \n \n" }, { "path": "franka_tools/setup.py", "content": "#!/usr/bin/env python\nfrom distutils.core import setup\nfrom catkin_pkg.python_setup import generate_distutils_setup\n\nd = generate_distutils_setup()\nd['packages'] = ['franka_tools']\nd['package_dir'] = {'': 'src'}\n\nsetup(**d)\n" }, { "path": "franka_tools/src/franka_tools/__init__.py", "content": "# /***************************************************************************\n# Copyright (c) 2019-2021, Saif Sidhik\n \n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n\n# http://www.apache.org/licenses/LICENSE-2.0\n\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n# **************************************************************************/\n\nfrom .controller_param_config_client import ControllerParamConfigClient\nfrom .frames_interface import FrankaFramesInterface\nfrom .joint_trajectory_action_client import JointTrajectoryActionClient\nfrom .controller_manager_interface import FrankaControllerManagerInterface\nfrom .collision_behaviour_interface import CollisionBehaviourInterface" }, { "path": "franka_tools/src/franka_tools/collision_behaviour_interface.py", "content": "# /***************************************************************************\n\n# \n# @package: franka_tools\n# @metapackage: franka_ros_interface\n# @author: Saif Sidhik \n# \n\n# **************************************************************************/\n\n# /***************************************************************************\n# Copyright (c) 2019-2021, Saif Sidhik\n \n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n\n# http://www.apache.org/licenses/LICENSE-2.0\n\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n# **************************************************************************/\nimport rospy\nfrom franka_msgs.srv import SetForceTorqueCollisionBehavior\n\nDEFAULT_VALUES = {\n 'torque_acc_lower' : [20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0],\n 'torque_acc_upper' : [20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0],\n 'torque_lower' : [20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0],\n 'torque_upper' : [20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0],\n 'force_acc_lower' : [20.0, 20.0, 20.0, 25.0, 25.0, 25.0],\n 'force_acc_upper' : [20.0, 20.0, 20.0, 25.0, 25.0, 25.0],\n 'force_lower' : [5.0, 5.0, 5.0, 25.0, 25.0, 25.0],\n 'force_upper' : [20.0, 20.0, 20.0, 25.0, 25.0, 25.0] # NOTE: COLLISION VALUES HAVE TO BE HIGHER THAN CONTACT THRESHOLDS (I.E. 'UPPER' THRESHOLDS HAVE TO BE HIGHER THAN 'LOWER' THRESHOLDS) FOR DETECTING COLLISIONS CORRECTLY. THE ROBOT DETECTS THE HIGHER VALUE OF THE TWO AS COLLISION.\n} \n\nclass CollisionBehaviourInterface(object):\n\n \"\"\"\n Helper class to set collision and contact thresholds at cartesian and joint levels.\n (This class has no 'getter' functions to access the currently set collision behaviour valures.)\n \"\"\"\n\n def __init__(self):\n\n rospy.loginfo(\"Waiting for collision behaviour services...\")\n rospy.wait_for_service(\"/franka_ros_interface/franka_control/set_force_torque_collision_behavior\", timeout = 1.0)\n self._ft_collision_behaviour_handle = rospy.ServiceProxy(\"/franka_ros_interface/franka_control/set_force_torque_collision_behavior\", SetForceTorqueCollisionBehavior)\n self._full_collision_behaviour_handle = rospy.ServiceProxy(\"/franka_ros_interface/franka_control/set_full_collision_behavior\", SetForceTorqueCollisionBehavior)\n\n rospy.loginfo(\"Collision behaviour services found.\")\n\n def set_ft_contact_collision_behaviour(self, torque_lower = None, torque_upper = None, force_lower = None, force_upper = None):\n \"\"\"\n :return: True if service call successful, False otherwise\n :rtype: bool\n :param torque_lower: Joint torque threshold for contact detection\n :type torque_lower: [float] size 7\n :param torque_upper: Joint torque threshold for collision (robot motion stops if violated)\n :type torque_upper: [float] size 7\n :param force_lower: Cartesian force threshold for contact detection [x,y,z,R,P,Y]\n :type force_lower: [float] size 6\n :param force_upper: Cartesian force threshold for collision detection [x,y,z,R,P,Y] (robot motion stops if violated)\n :type force_upper: [float] size 6\n \"\"\"\n\n if torque_lower is None:\n torque_lower = DEFAULT_VALUES['torque_lower']\n if torque_upper is None:\n torque_upper = DEFAULT_VALUES['torque_upper']\n if force_lower is None:\n force_lower = DEFAULT_VALUES['force_lower']\n if force_upper is None:\n force_upper = DEFAULT_VALUES['force_upper']\n\n try:\n response = self._ft_collision_behaviour_handle(lower_torque_thresholds_nominal = torque_lower,\n upper_torque_thresholds_nominal = torque_upper,\n lower_force_thresholds_nominal = force_lower,\n upper_force_thresholds_nominal = force_upper)\n\n rospy.loginfo(\"CollisionBehaviourInterface: Collision behaviour change request: %s. \\n\\tDetails: %s\"%(\"Success\" if response.success else \"Failed!\", response.error))\n\n except rospy.ServiceException as e:\n rospy.logwarn(\"CollisionBehaviourInterface: Collision behaviour change service call failed: %s\"%e)\n return False\n\n def set_force_threshold_for_contact(self, cartesian_force_values):\n \"\"\"\n :return: True if service call successful, False otherwise\n :rtype: bool\n :param cartesian_force_values: Cartesian force threshold for contact detection [x,y,z,R,P,Y]\n :type cartesian_force_values: [float] size 6\n \"\"\"\n return self.set_ft_contact_collision_behaviour(force_lower = cartesian_force_values)\n\n def set_force_threshold_for_collision(self, cartesian_force_values):\n \"\"\"\n :return: True if service call successful, False otherwise\n :rtype: bool\n :param cartesian_force_values: Cartesian force threshold for collision detection [x,y,z,R,P,Y] (robot motion stops if violated)\n :type cartesian_force_values: [float] size 6\n \"\"\"\n return self.set_ft_contact_collision_behaviour(force_upper = cartesian_force_values)\n\n def set_collision_threshold(self, joint_torques = None, cartesian_forces = None):\n \"\"\"\n :return: True if service call successful, False otherwise\n :rtype: bool\n :param joint_torques: Joint torque threshold for collision (robot motion stops if violated)\n :type joint_torques: [float] size 7\n :param cartesian_forces: Cartesian force threshold for collision detection [x,y,z,R,P,Y] (robot motion stops if violated)\n :type cartesian_forces: [float] size 6\n \"\"\"\n return self.set_ft_contact_collision_behaviour(torque_upper = joint_torques, force_upper = cartesian_forces)\n\n def set_contact_threshold(self, joint_torques = None, cartesian_forces = None):\n \"\"\"\n :return: True if service call successful, False otherwise\n :rtype: bool\n :param joint_torques: Joint torque threshold for identifying as contact\n :type joint_torques: [float] size 7\n :param cartesian_forces: Cartesian force threshold for identifying as contact\n :type cartesian_forces: [float] size 6\n \"\"\"\n return self.set_ft_contact_collision_behaviour(torque_lower = joint_torques, force_lower = cartesian_forces)\n" }, { "path": "franka_tools/src/franka_tools/controller_manager_interface.py", "content": "# /***************************************************************************\n\n# \n# @package: franka_tools\n# @metapackage: franka_ros_interface\n# @author: Saif Sidhik \n# \n\n# **************************************************************************/\n\n# /***************************************************************************\n# Copyright (c) 2019-2021, Saif Sidhik\n \n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n\n# http://www.apache.org/licenses/LICENSE-2.0\n\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n# **************************************************************************/\n\nimport rospy\nimport numpy as np\nfrom copy import deepcopy\nfrom controller_manager_msgs.msg import ControllerState\nfrom controller_manager_msgs.srv import *\nimport socket\nfrom franka_core_msgs.msg import JointControllerStates\n\nfrom franka_tools import ControllerParamConfigClient\n\nfrom controller_manager_msgs.utils import ( ControllerLister, \n get_rosparam_controller_names)\n\ndef _resolve_controllers_ns(cm_ns):\n \"\"\"\n Resolve the namespace containing controller configurations from that of\n the controller manager.\n Controllers are assumed to live one level above the controller\n manager, e.g.\n >>> _resolve_controller_ns('/path/to/controller_manager')\n '/path/to'\n In the particular case in which the controller manager is not\n namespaced, the controller is assumed to live in the root namespace\n >>> _resolve_controller_ns('/')\n '/'\n >>> _resolve_controller_ns('')\n '/'\n :param cm_ns Controller manager namespace (can be an empty string)\n :type cm_ns str\n :return: Controllers namespace\n :rtype: str\n \"\"\"\n ns = cm_ns.rsplit('/', 1)[0]\n if not ns:\n ns += '/'\n return ns\n\n\ndef _append_ns(in_ns, suffix):\n \"\"\"\n Append a sub-namespace (suffix) to the input namespace\n :param in_ns Input namespace\n :type in_ns str\n :return: Suffix namespace\n :rtype: str\n \"\"\"\n ns = in_ns\n if ns[-1] != '/':\n ns += '/'\n ns += suffix\n return ns\n\n\ndef _rosparam_controller_type(ctrls_ns, ctrl_name):\n \"\"\"\n Get a controller's type from its ROS parameter server configuration\n :param ctrls_ns Namespace where controllers should be located\n :type ctrls_ns str\n :param ctrl_name Controller name\n :type ctrl_name str\n :return: Controller type\n :rtype: str\n \"\"\"\n type_param = _append_ns(ctrls_ns, ctrl_name) + '/type'\n return rospy.get_param(type_param)\n\ndef _get_controller_name_from_rosparam_server(rosparam_name):\n try:\n cname = rospy.get_param(rosparam_name)\n except KeyError:\n rospy.loginfo(\"FrankaControllerManagerInterface: cannot detect controller name\"\n \" under param {}\".format(rosparam_name))\n cname = ''\n except (socket.error, socket.gaierror):\n rospy.logerr(\"Failed to connect to the ROS parameter server!\\n\"\n \"Please check to make sure your ROS networking is \"\n \"properly configured:\\n\")\n sys.exit()\n\n return cname\n\nclass ControllerStateInfo:\n \"\"\"\n Class for creating a storage object for controller state published by the induvidual franka_ros_controllers.\n \"\"\"\n def __init__(self, controller_state_msg):\n self.controller_name = controller_state_msg.controller_name\n self.p = np.asarray([j.p for j in controller_state_msg.joint_controller_states])\n self.d = np.asarray([j.d for j in controller_state_msg.joint_controller_states])\n self.i = np.asarray([j.i for j in controller_state_msg.joint_controller_states])\n self.process_value = np.asarray([j.process_value for j in controller_state_msg.joint_controller_states])\n self.set_point = np.asarray([j.set_point for j in controller_state_msg.joint_controller_states])\n self.process_value_dot = np.asarray([j.process_value_dot for j in controller_state_msg.joint_controller_states])\n self.error = np.asarray([j.error for j in controller_state_msg.joint_controller_states])\n self.time_step = np.asarray([j.time_step for j in controller_state_msg.joint_controller_states])\n self.i_clamp = np.asarray([j.i_clamp for j in controller_state_msg.joint_controller_states])\n self.command = np.asarray([j.command for j in controller_state_msg.joint_controller_states])\n\nclass FrankaControllerManagerInterface(object):\n \"\"\"\n \n :type synchronous_pub: bool\n :param synchronous_pub: designates the JointCommand Publisher\n as Synchronous if True and Asynchronous if False.\n\n Synchronous Publishing means that all joint_commands publishing to\n the robot's joints will block until the message has been serialized\n into a buffer and that buffer has been written to the transport\n of every current Subscriber. This yields predicable and consistent\n timing of messages being delivered from this Publisher. However,\n when using this mode, it is possible for a blocking Subscriber to\n prevent the joint_command functions from exiting. Unless you need exact\n JointCommand timing, default to Asynchronous Publishing (False).\n\n :param ns: base namespace of interface ('frank_ros_interface'/'panda_simulator')\n :type ns: str\n\n :param sim: Flag specifying whether the robot is in simulation or not \n (can be obtained from :py:class:`franka_interface.RobotParams` instance)\n :type sim: bool\n \"\"\"\n\n def __init__(self, ns=\"franka_ros_interface\", synchronous_pub = False, sim = False):\n\n self._ns = ns if ns[0] == '/' else '/' + ns\n self._prefix = \"/controller_manager\"\n self._cm_ns = self._prefix\n self._service_names = [\"list_controllers\",\n \"unload_controller\",\n \"load_controller\",\n \"switch_controller\"]\n\n\n load_srv_name = self._cm_ns + \"/load_controller\"\n self._load_srv = rospy.ServiceProxy(load_srv_name,\n LoadController,\n persistent=True)\n unload_srv_name = self._cm_ns + \"/unload_controller\"\n self._unload_srv = rospy.ServiceProxy(unload_srv_name,\n UnloadController,\n persistent=True)\n switch_srv_name = self._cm_ns + \"/switch_controller\"\n self._switch_srv = rospy.ServiceProxy(switch_srv_name,\n SwitchController,\n persistent=True)\n\n list_srv_name = self._cm_ns + \"/list_controllers\"\n self._list_srv = rospy.ServiceProxy(list_srv_name,\n ListControllers,\n persistent=True)\n\n list_types_srv_name = self._cm_ns + \"/list_controller_types\"\n self._list_types_srv = rospy.ServiceProxy(list_types_srv_name,\n ListControllerTypes,\n persistent=True)\n\n self._in_sim = sim\n\n self._controller_lister = ControllerLister(self._cm_ns)\n\n self._controller_names_from_rosparam = {\n 'joint_position_controller': _get_controller_name_from_rosparam_server('/controllers_config/position_controller'),\n 'joint_velocity_controller': _get_controller_name_from_rosparam_server('/controllers_config/velocity_controller'),\n 'joint_torque_controller': _get_controller_name_from_rosparam_server('/controllers_config/torque_controller'),\n 'joint_trajectory_controller': _get_controller_name_from_rosparam_server('/controllers_config/trajectory_controller'),\n 'effort_joint_position_controller': _get_controller_name_from_rosparam_server('/controllers_config/impedance_controller'),\n 'default_controller': _get_controller_name_from_rosparam_server('/controllers_config/default_controller'),\n }\n\n\n if self._in_sim:\n self._non_motion_controllers = [self._ns[1:] + '/custom_franka_state_controller', self._ns[1:] + '/panda_gripper_controller', self._ns[1:] + '/effort_joint_gravity_controller', self._ns[1:] + '/joint_state_controller']\n else:\n self._non_motion_controllers = [self._ns[1:] + '/custom_franka_state_controller',self._ns[1:] + '/franka_state_controller']\n\n self._assert_one_active_controller()\n\n self._state_subscriber = rospy.Subscriber(\"%s/motion_controller/arm/joint_controller_states\" %(self._ns),\n JointControllerStates, self._on_controller_state, queue_size = 1,\n tcp_nodelay = True)\n\n self._param_config_clients = {}\n self._dont_start_config_client = False\n\n\n rospy.on_shutdown(self._clean_shutdown)\n\n def _clean_shutdown(self):\n\n if self._state_subscriber:\n self._state_subscriber.unregister()\n\n\n def _on_controller_state(self, msg):\n self._controller_state = deepcopy(ControllerStateInfo(msg))\n\n self._assert_one_active_controller()\n\n if self._current_controller and self._current_controller not in self._param_config_clients:\n self._param_config_clients[self._current_controller] = ControllerParamConfigClient(self._current_controller)\n self._param_config_clients[self._current_controller].start()\n\n\n def _assert_one_active_controller(self):\n ctrlr_list = self.list_active_controllers(only_motion_controllers=True)\n assert len(ctrlr_list) <= 1, \"FrankaControllerManagerInterface: More than one motion controller active!\"\n self._current_controller = ctrlr_list[0].name if len(ctrlr_list) > 0 else None\n\n\n def load_controller(self, name):\n \"\"\"\n Loads the specified controller\n\n :type name: str\n :param name: name of the controller to be loaded\n \"\"\"\n self._load_srv.call(LoadControllerRequest(name=name))\n\n def unload_controller(self, name):\n \"\"\"\n Unloads the specified controller\n\n :type name: str\n :param name: name of the controller to be unloaded\n \"\"\"\n self._unload_srv.call(UnloadControllerRequest(name=name))\n\n def start_controller(self, name):\n \"\"\"\n Starts the specified controller\n\n :type name: str\n :param name: name of the controller to be started\n \"\"\"\n assert len(self.list_active_controllers(only_motion_controllers=True)) == 0, \"FrankaControllerManagerInterface: One motion controller already active: %s. Stop this controller before activating another!\"%self._current_controller\n\n strict = SwitchControllerRequest.STRICT\n req = SwitchControllerRequest(start_controllers=[name],\n stop_controllers=[],\n strictness=strict)\n rospy.logdebug(\"FrankaControllerManagerInterface: Starting controller: %s\"%name)\n self._switch_srv.call(req)\n\n self._assert_one_active_controller()\n\n def get_controller_state(self):\n \"\"\"\n Get the status of the current controller, including set points, computed \n command, controller gains etc. See the ControllerStateInfo class (above) \n parameters for more info.\n \"\"\"\n return deepcopy(self._controller_state)\n\n def stop_controller(self, name):\n \"\"\"\n Stops the specified controller\n\n :type name: str\n :param name: name of the controller to be stopped\n \"\"\"\n strict = SwitchControllerRequest.STRICT\n req = SwitchControllerRequest(start_controllers=[],\n stop_controllers=[name],\n strictness=strict)\n rospy.logdebug(\"FrankaControllerManagerInterface: Stopping controller: %s\"%name)\n self._switch_srv.call(req)\n\n\n def list_loaded_controllers(self):\n \"\"\"\n :return: List of controller types associated to a controller manager\n namespace. Contains all loaded controllers, as returned by\n the `list_controller_types` service, plus uninitialized controllers with\n configurations loaded in the parameter server.\n :rtype: [str]\n \"\"\"\n req = ListControllersRequest()\n\n return self._list_srv.call(req)\n \n def list_controller_types(self):\n \"\"\"\n :return: List of controller types associated to a controller manager\n namespace. Contains both stopped/running/loaded controllers, as returned by\n the `list_controller_types` service, plus uninitialized controllers with\n configurations loaded in the parameter server.\n :rtype: [str]\n \"\"\"\n req = ListControllerTypesRequest()\n\n return self._list_types_srv.call(req)\n\n def list_controllers(self):\n \"\"\"\n :return: List of controllers associated to a controller manager\n namespace. Contains both stopped/running controllers, as returned by\n the `list_controllers` service, plus uninitialized controllers with\n configurations loaded in the parameter server.\n :rtype: [ControllerState obj]\n \"\"\"\n if not self._cm_ns:\n return []\n\n # Add loaded controllers first\n controllers = self._controller_lister()\n\n # Append potential controller configs found in the parameter server\n all_ctrls_ns = _resolve_controllers_ns(self._cm_ns)\n for name in get_rosparam_controller_names(all_ctrls_ns):\n\n add_ctrl = not any(name == ctrl.name for ctrl in controllers)\n if add_ctrl:\n type_str = _rosparam_controller_type(all_ctrls_ns, name)\n uninit_ctrl = ControllerState(name=name,\n type=type_str,\n state='uninitialized')\n controllers.append(uninit_ctrl)\n for c in controllers:\n if c.name[0] == '/':\n c.name = c.name[1:]\n return controllers\n\n def controller_dict(self):\n \"\"\"\n Get all controllers as dict\n\n :return: name of the controller to be stopped\n :rtype: dict {'controller_name': ControllerState}\n \"\"\"\n controllers = self.list_controllers()\n\n controller_dict = {}\n for c in controllers:\n controller_dict[c.name] = c\n\n return controller_dict\n\n def set_motion_controller(self, controller_name):\n \"\"\"\n Set the specified controller as the (only) motion controller\n \n :return: name of currently active controller (can be used to switch back to this later)\n :rtype: str\n :type controller_name: str\n :param controller_name: name of controller to start\n \"\"\"\n\n if controller_name.strip() == '':\n rospy.logwarn(\"FrankaControllerManagerInterface: Empty controller name in controller switch request. Ignoring.\")\n return self._current_controller\n\n if controller_name[0] == '/':\n controller_name = controller_name[1:]\n \n curr_ctrlr = self._current_controller\n switch_ctrl = (curr_ctrlr != controller_name)\n\n if switch_ctrl:\n active_controllers = self.list_active_controllers(only_motion_controllers = True)\n for ctrlr in active_controllers:\n self.stop_controller(ctrlr.name)\n rospy.sleep(0.5)\n\n if not self.is_loaded(controller_name):\n self.load_controller(controller_name)\n\n self.start_controller(controller_name)\n else:\n rospy.logdebug(\"FrankaControllerManagerInterface: Controller '{0}' already active. Not switching.\".format(controller_name))\n\n return curr_ctrlr\n\n\n def is_running(self, controller_name):\n \"\"\"\n Check if the given controller is running.\n\n :type controller_name: str\n :param controller_name: name of controller whose status is to be checked\n :return: True if controller is running, False otherwise\n :rtype: bool\n \"\"\"\n controllers = self.controller_dict()\n\n ctrl_state = controllers.get(controller_name,None)\n\n return ctrl_state is not None and ctrl_state.state==\"running\"\n\n def is_loaded(self, controller_name):\n \"\"\"\n Check if the given controller is loaded.\n\n :type controller_name: str\n :param controller_name: name of controller whose status is to be checked\n :return: True if controller is loaded, False otherwise\n :rtype: bool\n \"\"\"\n controllers = self.controller_dict()\n\n ctrl_state = controllers.get(controller_name,None)\n\n return ctrl_state is not None and ctrl_state.state!=\"uninitialized\"\n\n def list_motion_controllers(self):\n \"\"\"\n :return: List of motion controllers associated to a controller manager\n namespace. Contains both stopped/running controllers, as returned by\n the `list_controllers` service, plus uninitialized controllers with\n configurations loaded in the parameter server.\n :rtype: [ControllerState obj]\n \"\"\"\n motion_controllers = []\n for controller in self.list_controllers():\n if not controller.name in self._non_motion_controllers:\n motion_controllers.append(controller)\n\n return motion_controllers\n\n\n def list_active_controllers(self, only_motion_controllers = False):\n \"\"\"\n :return: List of active controllers associated to a controller manager\n namespace. Contains both stopped/running controllers, as returned by\n the `list_controllers` service, plus uninitialized controllers with\n configurations loaded in the parameter server.\n :rtype: [ControllerState obj]\n \n :param only_motion_controller: if True, only motion controllers are returned\n :type only_motion_controller: bool\n\n \"\"\"\n if only_motion_controllers:\n controllers = self.list_motion_controllers()\n else:\n controllers = self.list_controllers()\n \n active_controllers = []\n for controller in controllers:\n if controller.state == \"running\":\n active_controllers.append(controller)\n\n return active_controllers\n\n def list_active_controller_names(self, only_motion_controllers = False):\n \"\"\"\n :return: List of names active controllers associated to a controller manager\n namespace. \n :rtype: [str]\n \n :param only_motion_controller: if True, only motion controllers are returned\n :type only_motion_controller: bool\n\n \"\"\"\n return [c.name for c in self.list_active_controllers(only_motion_controllers = only_motion_controllers)]\n\n def get_controller_config_client(self, controller_name):\n \"\"\"\n :return: The parameter configuration client object associated with the specified\n controller\n :rtype: ControllerParamConfigClient obj (if None, returns False)\n \n :param controller_name: name of controller whose config client is required\n :type controller_name: str\n\n \"\"\"\n if controller_name in self._param_config_clients:\n return self._param_config_clients[controller_name]\n else:\n rospy.logwarn(\"FrankaControllerManagerInterface: No parameter configuration client available for controller {}\".format(controller_name))\n return False\n\n def get_current_controller_config_client(self):\n \"\"\"\n :return: The parameter configuration client object associated with the currently\n active controller\n :rtype: ControllerParamConfigClient obj (if None, returns False)\n \n :param controller_name: name of controller whose config client is required\n :type controller_name: str\n\n \"\"\"\n if self._current_controller is None:\n rospy.logwarn(\"FrankaControllerManagerInterface: No active controller!\")\n return False\n\n return self.get_controller_config_client(self._current_controller)\n\n\n def list_controller_names(self):\n \"\"\"\n :return: List of names all controllers associated to a controller manager\n namespace. \n :rtype: [str]\n \n :param only_motion_controller: if True, only motion controllers are returned\n :type only_motion_controller: bool\n\n \"\"\"\n return [c.name for c in self.list_controllers()]\n\n\n\n \"\"\"\n Properties that give the names of the controllers for each type.\n \"\"\"\n @property\n def joint_velocity_controller(self):\n \"\"\"\n :getter: Returns the name of joint velocity controller \n (defined in franka_ros_controllers, and specified \n in robot_config.yaml). Can be used for changing \n motion controller using \n :py:meth:`FrankaControllerManagerInterface.set_motion_controller`.\n :type: str\n \"\"\"\n return self._controller_names_from_rosparam['joint_velocity_controller'] \n @property\n def joint_position_controller(self):\n \"\"\"\n :getter: Returns the name of joint position controller \n (defined in franka_ros_controllers, and specified \n in robot_config.yaml). Can be used for changing \n motion controller using \n :py:meth:`FrankaControllerManagerInterface.set_motion_controller`.\n :type: str\n \"\"\"\n return self._controller_names_from_rosparam['joint_position_controller'] \n @property\n def joint_torque_controller(self):\n \"\"\"\n :getter: Returns the name of joint torque controller \n (defined in franka_ros_controllers, and specified \n in robot_config.yaml). Can be used for changing \n motion controller using \n :py:meth:`FrankaControllerManagerInterface.set_motion_controller`.\n :type: str\n \"\"\"\n return self._controller_names_from_rosparam['joint_torque_controller'] \n @property\n def joint_impedance_controller(self):\n \"\"\"\n :getter: Returns the name of joint impedance controller \n (defined in franka_ros_controllers, and specified \n in robot_config.yaml). Can be used for changing \n motion controller using \n :py:meth:`FrankaControllerManagerInterface.set_motion_controller`.\n :type: str\n \"\"\"\n return self._controller_names_from_rosparam['joint_impedance_controller'] \n @property\n def effort_joint_position_controller(self):\n \"\"\"\n :getter: Returns the name of effort-based joint position controller \n (defined in franka_ros_controllers, and specified \n in robot_config.yaml). Can be used for changing \n motion controller using \n :py:meth:`FrankaControllerManagerInterface.set_motion_controller`.\n :type: str\n \"\"\"\n return self._controller_names_from_rosparam['effort_joint_position_controller']\n @property\n def joint_trajectory_controller(self):\n \"\"\"\n :getter: Returns the name of joint trajectory controller \n (defined in franka_ros_controllers, and specified \n in robot_config.yaml). Can be used for changing \n motion controller using \n :py:meth:`FrankaControllerManagerInterface.set_motion_controller`.\n This controller exposes trajectory following service.\n :type: str\n \"\"\"\n if self._in_sim:\n return self.joint_position_controller\n return self._controller_names_from_rosparam['joint_trajectory_controller']\n\n @property\n def current_controller(self):\n \"\"\"\n :getter: Returns the name of currently active controller.\n :type: str\n \"\"\"\n self._assert_one_active_controller()\n if not self._current_controller:\n rospy.logwarn(\"FrankaControllerManagerInterface: No active controller!\")\n return self._current_controller\n\n @property\n def default_controller(self):\n \"\"\"\n :getter: Returns the name of the default controller for Franka ROS Interface.\n (specified in robot_config.yaml). Should ideally default to the same as\n :py:meth:`joint_trajectory_controller`.\n :type: str\n \"\"\"\n return self._controller_names_from_rosparam['default_controller']\n\n \n\n\nif __name__ == '__main__':\n from franka_tools import FrankaFramesInterface\n cmi = FrankaControllerManagerInterface()\n f = FrankaFramesInterface()\n\n\n\n\n" }, { "path": "franka_tools/src/franka_tools/controller_param_config_client.py", "content": "# /***************************************************************************\n\n# \n# @package: franka_tools\n# @metapackage: franka_ros_interface\n# @author: Saif Sidhik \n# \n\n# **************************************************************************/\n\n# /***************************************************************************\n# Copyright (c) 2019-2021, Saif Sidhik\n \n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n\n# http://www.apache.org/licenses/LICENSE-2.0\n\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n# **************************************************************************/\n\nimport rospy\n\nimport dynamic_reconfigure.client\n\n\nK_GAINS_KW = ['j1_k','j2_k','j3_k','j4_k','j5_k','j6_k','j7_k']\nD_GAINS_KW = ['j1_d','j2_d','j3_d','j4_d','j5_d','j6_d','j7_d']\n\n\nclass ControllerParamConfigClient:\n \"\"\"\n Interface class for updating dynamically configurable paramters of a controller.\n\n :param controller_name: The name of the controller.\n :type controller_name: str\n\n \"\"\"\n def __init__(self, controller_name):\n \"\"\"\n Initialisation: Client is not started yet.\n\n \"\"\"\n self._controller_name = controller_name if controller_name[0] != '/' else controller_name[1:]\n self._is_running = False\n\n @property\n def is_running(self):\n \"\"\"\n :return: True if client is running / server is unavailable; False otherwise\n :rtype: bool\n\n \"\"\"\n return self._is_running\n \n\n def start(self, timeout = 5):\n \"\"\"\n Start the dynamic_reconfigure client\n \n :param timeout: time to wait before giving up on service request\n :type timeout: float\n\n \"\"\"\n service_name = \"/{}/arm/controller_parameters_config\".format(self._controller_name)\n try:\n self._client = dynamic_reconfigure.client.Client(service_name, timeout=timeout, config_callback=self._log_update)\n except rospy.ROSException:\n rospy.logdebug(\"ControllerParamConfigClient: Could not find configuration server at {}\".format(service_name))\n self._is_running = True\n\n def _log_update(self, config): \n \"\"\"\n Optional callback to log parameter changes after each update request.\n\n \"\"\"\n k_gains = [config[n] for n in K_GAINS_KW]\n d_gains = [config[n] for n in D_GAINS_KW]\n\n rospy.logdebug(\"ControllerParamConfigClient: {controller_name} config set to \\n\\tSmoothing Param: {smoothing_param} \\n\\tJoint Stiffness: {K_gains} \\n\\tJoint Damping: {D_gains}\".format(controller_name=self._controller_name, smoothing_param = config['position_joint_delta_filter'], K_gains = k_gains, D_gains = d_gains))\n\n\n def update_config(self, **kwargs):\n \"\"\"\n Update the config in the server using the provided keyword arguments.\n \n :param kwargs: These are keyword arguments matching the parameter names\n in config file: franka_ros_controllers/cfg/joint_controller_params.cfg\n\n \"\"\"\n self._client.update_configuration(kwargs)\n\n\n def set_controller_gains(self, k_gains, d_gains = None):\n \"\"\"\n Update the stiffness and damping parameters of the joints for the current controller.\n \n :param k_gains: joint stiffness parameters (should be within limits specified in \n franka documentation; same is also set \n in franka_ros_controllers/cfg/joint_controller_params.cfg)\n :type k_gains: [float]\n :param d_gains: joint damping parameters (should be within limits specified in \n franka documentation; same is also set \n in franka_ros_controllers/cfg/joint_controller_params.cfg)\n :type d_gains: [float]\n\n \"\"\"\n assert len(k_gains) == 7, \"ControllerParamConfigClient: k_gains argument should be of length 7!\"\n\n config = {}\n for i, k_val in enumerate(k_gains):\n config[K_GAINS_KW[i]] = k_val\n\n if d_gains:\n assert len(k_gains) == 7, \"ControllerParamConfigClient: d_gains argument should be of length 7!\"\n\n for j, d_val in enumerate(d_gains):\n config[D_GAINS_KW[j]] = d_val\n\n rospy.logdebug(\"Config: {}\".format(config))\n\n self.update_config(**config)\n\n\n def set_joint_motion_smoothing_parameter(self, value):\n \"\"\"\n Update the joint motion smoothing parameter (only valid for \n position_joint_position_controller).\n \n :param value: smoothing factor (should be within limit set \n in franka_ros_controllers/cfg/joint_controller_params.cfg)\n :type value: [float]\n\n \"\"\"\n self.update_config(position_joint_delta_filter = value)\n\n def get_joint_motion_smoothing_parameter(self, timeout = 5):\n \"\"\"\n :return: the currently set value for the joint position smoothing parameter from \n the server.\n :rtype: float\n \n :param timeout: time to wait before giving up on service request\n :type timeout: float\n\n \"\"\"\n return self.get_config(timeout = timeout)['position_joint_delta_filter']\n\n\n def get_config(self, timeout = 5):\n \"\"\"\n :return: the currently set values for all paramters from the server\n :rtype: dict {str : float}\n \n :param timeout: time to wait before giving up on service request\n :type timeout: float\n\n \"\"\"\n return self._client.get_configuration(timeout = timeout)\n\n def get_controller_gains(self, timeout = 5):\n \"\"\"\n :return: the currently set values for controller gains from the server\n :rtype: ( [float], [float] )\n \n :param timeout: time to wait before giving up on service request\n :type timeout: float\n\n \"\"\"\n config = self.get_config(timeout = timeout)\n\n k_gains = []\n\n for k_val_ in K_GAINS_KW:\n if k_val_ in config:\n k_gains.append(config[k_val_])\n else:\n rospy.logwarn(\"ControllerParamConfigClient: Could not find K gain {} in server\".format(k_val_))\n return False, False\n\n d_gains = []\n\n for d_val_ in D_GAINS_KW:\n if d_val_ in config:\n d_gains.append(config[d_val_])\n else:\n rospy.logwarn(\"ControllerParamConfigClient: Could not find D gain {} in server\".format(d_val_))\n return k_gains, False\n\n return k_gains, d_gains\n \n\n\n def get_parameter_descriptions(self, timeout = 5):\n \"\"\"\n :return: the description of each parameter as defined in the cfg\n file from the server.\n :rtype: dict {str : str}\n \n :param timeout: time to wait before giving up on service request\n :type timeout: float\n\n \"\"\"\n return self._client.get_parameter_descriptions(timeout = timeout)\n\n\nif __name__ == \"__main__\":\n rospy.init_node(\"dynamic_client\")\n\n # client = dynamic_reconfigure.client.Client(\"dynamic_tutorials\", timeout=30, config_callback=callback)\n\n" }, { "path": "franka_tools/src/franka_tools/frames_interface.py", "content": "# /***************************************************************************\n\n#\n# @package: franka_tools\n# @metapackage: franka_ros_interface\n# @author: Saif Sidhik \n#\n\n# **************************************************************************/\n\n# /***************************************************************************\n# Copyright (c) 2019-2021, Saif Sidhik\n\n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n\n# http://www.apache.org/licenses/LICENSE-2.0\n\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n# **************************************************************************/\n\nimport tf2_ros as tf\nimport rospy\nimport quaternion\nimport numpy as np\nimport franka_dataflow\nfrom franka_msgs.srv import SetEEFrame, SetKFrame\n\nfrom collections import namedtuple\n_FRAME_NAMES = namedtuple('Constants', ['EE_FRAME', 'K_FRAME'])\nDEFAULT_TRANSFORMATIONS = _FRAME_NAMES([1, 0.0, 0.0, 0.0, 0.0,\n 1, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0], # EE_FRAME\n [1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,\n 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0] # K_FRAME\n ) # identity matrices by default\n\n\nclass FrankaFramesInterface(object):\n \"\"\"\n Helper class to retrieve and set EE frames.\n\n .. note:: The current (realtime) transformation of the end-effector frame and stiffness frame has to be updated externally each time franka states is updated. All of this is done automatically within the ArmInterface class, so use the instance of FrankaFramesInterface from the ArmInterface object using :py:meth:`get_frames_interface` for ease.\n\n .. note:: All controllers have to be unloaded before switching frames. This has to be done externally (also automatically handled in ArmInterface class).\n\n .. note:: It is best to provide all transformation matrices using numpy arrays. If providing as flattened 1D list, make sure it is in column major format.\n\n \"\"\"\n\n def __init__(self):\n\n self._current_EE_frame_transformation = None # NE_T_EE\n self._current_K_frame_transformation = None # EE_T_K\n\n def set_EE_frame(self, frame):\n \"\"\"\n Set new EE frame based on the transformation given by 'frame', which is the \n transformation matrix defining the new desired EE frame with respect to the nominal end-effector frame (NE_T_EE).\n\n :type frame: [float (16,)] / np.ndarray (4x4)\n :param frame: transformation matrix of new EE frame wrt nominal end-effector frame (if list, should be column major)\n :rtype: bool\n :return: success status of service request\n \"\"\"\n frame = self._assert_frame_validity(frame)\n\n return self._request_setEE_service(frame)\n\n def _update_frame_data(self, EE_frame_transformation, K_frame_transformation):\n \"\"\"\n The callback function used by the PandaArm class to update the current frame transformations NE_T_EE and EE_T_K.\n \"\"\"\n assert len(\n EE_frame_transformation) == 16, \"FrankaFramesInterface: Current EE frame transformation could not be retrieved!\"\n self._current_EE_frame_transformation = EE_frame_transformation\n\n assert len(\n K_frame_transformation) == 16, \"FrankaFramesInterface: Current K frame transformation could not be retrieved!\"\n self._current_K_frame_transformation = K_frame_transformation\n\n def _assert_frame_validity(self, frame):\n\n if isinstance(frame, np.ndarray):\n if frame.shape[0] == frame.shape[1] == 4:\n frame = frame.flatten('F').tolist()\n else:\n raise ValueError(\n \"Invalid shape for transformation matrix numpy array\")\n else:\n assert len(\n frame) == 16, \"Invalid number of elements in transformation matrix. Should have 16 elements.\"\n\n return frame\n\n def get_link_tf(self, frame_name, timeout=5.0, parent='panda_NE'):\n \"\"\"\n Get :math:`4\\times 4` transformation matrix of a frame with respect to another.\n :return: :math:`4\\times 4` transformation matrix\n :rtype: np.ndarray\n :param frame_name: Name of the child frame from the TF tree\n :type frame_name: str\n :param parent: Name of parent frame (default: 'panda_NE', the default nominal end-effector frame set in Desk)\n :type parent: str\n \"\"\"\n tfBuffer = tf.Buffer()\n listener = tf.TransformListener(tfBuffer)\n err = \"FrankaFramesInterface: Error while looking up transform from Flange frame to link frame %s\" % frame_name\n\n def body():\n try:\n tfBuffer.lookup_transform(parent, frame_name, rospy.Time(0))\n except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):\n return False\n return True\n\n franka_dataflow.wait_for(\n lambda: body(), timeout=timeout, raise_on_error=True, timeout_msg=err)\n\n trans_stamped = tfBuffer.lookup_transform(\n parent, frame_name, rospy.Time(0))\n t = [trans_stamped.transform.translation.x,\n trans_stamped.transform.translation.y, trans_stamped.transform.translation.z]\n q = trans_stamped.transform.rotation\n\n rot = np.quaternion(q.w, q.x, q.y, q.z)\n\n rot = quaternion.as_rotation_matrix(rot)\n\n trans_mat = np.eye(4)\n\n trans_mat[:3, :3] = rot\n trans_mat[:3, 3] = np.array(t)\n\n return trans_mat\n\n def frames_are_same(self, frame1, frame2):\n \"\"\"\n :return: True if two transformation matrices are equal\n :rtype: bool\n :param frame1: 4x4 transformation matrix representing frame1\n :type frame1: np.ndarray (shape: [4,4]), or list (flattened column major 4x4)\n :param frame2: 4x4 transformation matrix representing frame2\n :type frame2: np.ndarray (shape: [4,4]), or list (flattened column major 4x4)\n \"\"\"\n frame1 = self._assert_frame_validity(frame1)\n frame2 = self._assert_frame_validity(frame2)\n\n return np.array_equal(np.asarray(frame1), np.asarray(frame2))\n\n def EE_frame_already_set(self, frame):\n \"\"\"\n :return: True if the requested frame is already the current EE frame\n :rtype: bool\n :param frame: 4x4 transformation matrix representing frame\n :type frame: np.ndarray (shape: [4,4]), or list (flattened column major 4x4)\n \"\"\"\n return self.frames_are_same(frame, self._current_EE_frame_transformation)\n\n def set_EE_at_frame(self, frame_name, timeout=5.0):\n \"\"\"\n Set new EE frame to the same frame as the link frame given by 'frame_name'.\n Only 'panda_link7', 'panda_link8', 'panda_hand', 'panda_NE', and 'panda_EE' \n are valid frames for this operation. Other frames will not produce required\n results.\n Motion controllers should be stopped for switching.\n\n :type frame_name: str \n :param frame_name: desired tf frame name in the tf tree\n :rtype: [bool, str]\n :return: [success status of service request, error msg if any]\n \"\"\"\n\n return self.set_EE_frame(self.get_link_tf(frame_name, timeout))\n\n def get_EE_frame(self, as_mat=False):\n \"\"\"\n Get current EE frame transformation matrix in flange frame\n\n :type as_mat: bool\n :param as_mat: if True, return np array, else as list\n :rtype: [float (16,)] / np.ndarray (4x4) \n :return: transformation matrix of EE frame wrt flange frame (column major)\n \"\"\"\n return self._current_EE_frame_transformation if not as_mat else np.asarray(self._current_EE_frame_transformation).reshape(4, 4, order='F')\n\n def reset_EE_frame(self):\n \"\"\"\n Reset EE frame to default. (defined by DEFAULT_TRANSFORMATIONS.EE_FRAME global variable defined above)\n By default, this is identity aligning it with the nominal end-effector frame.\n\n :rtype: bool\n :return: success status of service request\n \"\"\"\n return self.set_EE_frame(frame=DEFAULT_TRANSFORMATIONS.EE_FRAME)\n\n def EE_frame_is_reset(self):\n assert self._current_EE_frame_transformation is not None, \"FrankaFramesInterface: Current EE Frame is not known.\"\n return list(self._current_EE_frame_transformation) == list(DEFAULT_TRANSFORMATIONS.EE_FRAME)\n\n def _request_setEE_service(self, trans_mat):\n\n rospy.wait_for_service(\n '/franka_ros_interface/franka_control/set_EE_frame')\n try:\n service_handle = rospy.ServiceProxy(\n '/franka_ros_interface/franka_control/set_EE_frame', SetEEFrame)\n response = service_handle(F_T_EE=trans_mat)\n rospy.loginfo(\"Set EE Frame Request Status: %s. \\n\\tDetails: %s\" % (\n \"Success\" if response.success else \"Failed!\", response.error))\n return response.success\n except rospy.ServiceException as e:\n rospy.logwarn(\"Set EE Frame Request: Service call failed: %s\" % e)\n return False\n\n def set_K_frame(self, frame):\n \"\"\"\n Set new K frame based on the transformation given by 'frame', which is the \n transformation matrix defining the new desired K frame with respect to the EE frame.\n\n :type frame: [float (16,)] / np.ndarray (4x4) \n :param frame: transformation matrix of new K frame wrt EE frame\n :rtype: bool\n :return: success status of service request\n \"\"\"\n frame = self._assert_frame_validity(frame)\n\n return self._request_setK_service(frame)\n\n def set_K_at_frame(self, frame_name, timeout=5.0):\n \"\"\"\n Set new K frame to the same frame as the link frame given by 'frame_name'\n Only 'panda_link7', 'panda_link8', 'panda_hand', 'panda_NE', and 'panda_EE' \n are valid frames for this operation. Other frames will not produce required\n results.\n Motion controllers should be stopped for switching.\n\n :type frame_name: str \n :param frame_name: desired tf frame name in the tf tree\n :rtype: [bool, str]\n :return: [success status of service request, error msg if any]\n \"\"\"\n return self.set_K_frame(self.get_link_tf(frame_name, timeout, parent=\"panda_EE\"))\n \n def get_K_frame(self, as_mat=False):\n \"\"\"\n Get current K frame transformation matrix in EE frame\n\n :type as_mat: bool\n :param as_mat: if True, return np array, else as list\n :rtype: [float (16,)] / np.ndarray (4x4) \n :return: transformation matrix of K frame wrt EE frame\n \"\"\"\n return self._current_K_frame_transformation if not as_mat else np.asarray(self._current_K_frame_transformation).reshape(4, 4, order='F')\n\n def reset_K_frame(self):\n \"\"\"\n Reset K frame to default. (defined by DEFAULT_K_FRAME global variable defined above)\n By default this resets to align the stiffness frame to the end-effector frame.\n\n :rtype: bool\n :return: success status of service request\n \"\"\"\n\n return self.set_K_frame(frame=DEFAULT_TRANSFORMATIONS.K_FRAME)\n\n def K_frame_is_reset(self):\n assert self._current_K_frame_transformation is not None, \"FrankaFramesInterface: Current K Frame is not known.\"\n return list(self._current_K_frame_transformation) == list(DEFAULT_TRANSFORMATIONS.K_FRAME)\n\n def _request_setK_service(self, trans_mat):\n rospy.wait_for_service(\n '/franka_ros_interface/franka_control/set_K_frame')\n try:\n service_handle = rospy.ServiceProxy(\n '/franka_ros_interface/franka_control/set_K_frame', SetKFrame)\n response = service_handle(EE_T_K=trans_mat)\n rospy.loginfo(\"Set K Frame Request Status: %s. \\n\\tDetails: %s\" % (\n \"Success\" if response.success else \"Failed!\", response.error))\n return response.success\n except rospy.ServiceException as e:\n rospy.logwarn(\"Set K Frame Request: Service call failed: %s\" % e)\n return False\n\n\nif __name__ == '__main__':\n\n rospy.init_node(\"test\")\n\n ee_setter = FrankaFramesInterface()\n\n # ee_setter.set_EE_frame([1,0,0,0,0,1,0,0,0,0,1,0,0,0,0,1])\n" }, { "path": "franka_tools/src/franka_tools/joint_trajectory_action_client.py", "content": "# /***************************************************************************\n\n# \n# @package: franka_tools\n# @metapackage: franka_ros_interface\n# @author: Saif Sidhik \n# \n\n# **************************************************************************/\n\n# /***************************************************************************\n# Copyright (c) 2019-2021, Saif Sidhik\n \n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n\n# http://www.apache.org/licenses/LICENSE-2.0\n\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n# **************************************************************************/\n\nimport rospy\nimport sys\nimport actionlib\nfrom copy import copy\nfrom control_msgs.msg import (\n FollowJointTrajectoryAction,\n FollowJointTrajectoryGoal,\n)\nfrom trajectory_msgs.msg import (\n JointTrajectoryPoint,\n)\n\n\nclass JointTrajectoryActionClient(object):\n \"\"\"\n To use this class, the currently active controller for the franka robot should be\n the \"joint_position_trajectory_controller\". This can be set using instance of\n :py:class:`franka_tools.FrankaControllerManagerInterface`.\n \"\"\"\n def __init__(self, joint_names, controller_name = \"position_joint_trajectory_controller\"):\n self._joint_names = joint_names\n\n self._client = actionlib.SimpleActionClient(\"/%s/follow_joint_trajectory\"%(controller_name),\n FollowJointTrajectoryAction,\n )\n self._goal = FollowJointTrajectoryGoal()\n self._goal_time_tolerance = rospy.Time(0.1)\n self._goal.goal_time_tolerance = self._goal_time_tolerance\n server_up = self._client.wait_for_server(timeout=rospy.Duration(3.0))\n if not server_up:\n rospy.logerr(\"Timed out waiting for Joint Trajectory\"\n \" Action Server to connect. Start the action server\"\n \" before running example.\")\n rospy.signal_shutdown(\"Timed out waiting for Action Server\")\n sys.exit(1)\n self.clear()\n\n def add_point(self, positions, time, velocities = None):\n \"\"\"\n Add a waypoint to the trajectory.\n\n :param positions: target joint positions\n :type positions: [float]*7\n :param time: target time in seconds from the start of trajectory \n to reach the specified goal \n :type time: float\n :param velocities: goal velocities for joints (give atleast 0.0001)\n :type velocities: [float]*7\n\n .. note:: Velocities should be greater than zero (done by default) for\n smooth motion.\n\n \"\"\"\n point = JointTrajectoryPoint()\n point.positions = copy(positions)\n if velocities is None:\n point.velocities = [0.0001 for n in positions]\n else:\n point.velocities = copy(velocities)\n point.time_from_start = rospy.Duration(time)\n self._goal.trajectory.points.append(point)\n\n def start(self):\n \"\"\"\n Execute previously defined trajectory.\n \"\"\"\n self._goal.trajectory.header.stamp = rospy.Time.now()\n self._client.send_goal(self._goal)\n\n def stop(self):\n \"\"\"\n Stop currently executing trajectory.\n \"\"\"\n self._client.cancel_goal()\n\n def wait(self, timeout=15.0):\n \"\"\"\n Wait for trajectory execution result.\n\n :param timeout: timeout before cancelling wait\n :type timeout: float\n \"\"\"\n self._client.wait_for_result(timeout=rospy.Duration(timeout))\n\n def result(self):\n \"\"\"\n Get result from trajectory action server\n \"\"\"\n return self._client.get_result()\n\n def clear(self):\n \"\"\"\n Clear all waypoints from the current trajectory definition.\n \"\"\"\n self._goal = FollowJointTrajectoryGoal()\n self._goal.goal_time_tolerance = self._goal_time_tolerance\n self._goal.trajectory.joint_names = self._joint_names\n\n\nif __name__ == '__main__':\n \n pass" } ]