Repository: justagist/franka_ros_interface Branch: master Commit: f1f3649a4b03 Files: 85 Total size: 390.0 KB Directory structure: gitextract_jpas59zs/ ├── .github/ │ └── workflows/ │ └── auto-comment.yml ├── .gitignore ├── .travis.yml ├── CMakeLists.txt ├── LICENSE ├── NOTICE ├── README.md ├── cmake/ │ └── ClangTools.cmake ├── franka.sh ├── franka_common/ │ └── franka_core_msgs/ │ ├── CMakeLists.txt │ ├── msg/ │ │ ├── EndPointState.msg │ │ ├── JointCommand.msg │ │ ├── JointControllerStates.msg │ │ ├── JointLimits.msg │ │ └── RobotState.msg │ └── package.xml ├── franka_interface/ │ ├── CMakeLists.txt │ ├── README.md │ ├── config/ │ │ ├── basic_controllers.yaml │ │ └── robot_config.yaml │ ├── include/ │ │ └── franka_interface/ │ │ ├── motion_controller_interface.h │ │ └── robot_state_controller.h │ ├── launch/ │ │ └── interface.launch │ ├── package.xml │ ├── scripts/ │ │ ├── enable_robot.py │ │ ├── move_to_neutral.py │ │ └── simple_gripper.py │ ├── setup.py │ ├── src/ │ │ ├── franka_control_node.cpp │ │ ├── franka_dataflow/ │ │ │ ├── __init__.py │ │ │ ├── getch.py │ │ │ └── wait_for.py │ │ ├── franka_interface/ │ │ │ ├── __init__.py │ │ │ ├── arm.py │ │ │ ├── gripper.py │ │ │ ├── robot_enable.py │ │ │ └── robot_params.py │ │ ├── motion_controller_interface.cpp │ │ └── robot_state_controller.cpp │ ├── state_controller_plugin.xml │ └── tests/ │ ├── demo_interface.py │ ├── demo_joint_positions_keyboard.py │ └── test_zeroG.py ├── franka_moveit/ │ ├── CMakeLists.txt │ ├── README.md │ ├── config/ │ │ └── moveit_demo.rviz │ ├── launch/ │ │ └── demo_moveit.launch │ ├── package.xml │ ├── scripts/ │ │ └── create_demo_planning_scene.py │ ├── setup.py │ ├── src/ │ │ └── franka_moveit/ │ │ ├── __init__.py │ │ ├── extended_planning_scene_interface.py │ │ ├── movegroup_interface.py │ │ └── utils.py │ └── tests/ │ ├── test_movegroup_interface.py │ └── test_scene_interface.py ├── franka_ros_controllers/ │ ├── CMakeLists.txt │ ├── README.md │ ├── cfg/ │ │ └── joint_controller_params.cfg │ ├── config/ │ │ └── ros_controllers.yaml │ ├── controller_plugins.xml │ ├── include/ │ │ └── franka_ros_controllers/ │ │ ├── effort_joint_impedance_controller.h │ │ ├── effort_joint_position_controller.h │ │ ├── effort_joint_torque_controller.h │ │ ├── position_joint_position_controller.h │ │ └── velocity_joint_velocity_controller.h │ ├── package.xml │ ├── scripts/ │ │ └── test_controller.py │ └── src/ │ ├── effort_joint_impedance_controller.cpp │ ├── effort_joint_position_controller.cpp │ ├── effort_joint_torque_controller.cpp │ ├── position_joint_position_controller.cpp │ └── velocity_joint_velocity_controller.cpp ├── franka_ros_interface/ │ ├── CMakeLists.txt │ └── package.xml └── franka_tools/ ├── CMakeLists.txt ├── README.md ├── package.xml ├── setup.py └── src/ └── franka_tools/ ├── __init__.py ├── collision_behaviour_interface.py ├── controller_manager_interface.py ├── controller_param_config_client.py ├── frames_interface.py └── joint_trajectory_action_client.py ================================================ FILE CONTENTS ================================================ ================================================ FILE: .github/workflows/auto-comment.yml ================================================ name: Auto Comment on: [issues, pull_request] jobs: run: runs-on: ubuntu-latest steps: - uses: wow-actions/auto-comment@v1 with: GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }} issuesOpened: | Hi @{{ author }}, thank you for using this repository. Unfortunately, I am unable to maintain this package any more, sorry! If you are interested in contributing to this repository or are willing to maintain it, please contact me at mail@saifsidhik.page. -- Saif pullRequestOpened: | Hi @{{ author }}, Thank you for the pull request. Unfortunately, I am unable to maintain this package any more, sorry! Please feel free to maintain a working fork of the repo. If you are interested in contributing to this repository or are willing to maintain it, please contact me at mail@saifsidhik.page. -- Saif ================================================ FILE: .gitignore ================================================ *.py[cod] __pycache__ *$py.class # C extensions *.so # Packages *.egg *.egg-info dist build eggs parts bin var sdist develop-eggs .installed.cfg lib lib64 # Installer logs pip-log.txt # Unit test / coverage reports .coverage .tox nosetests.xml # Translations *.mo # Mr Developer .mr.developer.cfg .project .cproject cmake_install.cmake .pydevproject *.swp *.swo CATKIN_IGNORE catkin catkin_generated devel cpp docs_src doc_src docs_build msg_gen srv_gen *.smoketest *.cfgc *_msgs/src/ */src/**/cfg */*/src/**/* franka_interface/scripts/test.py ================================================ FILE: .travis.yml ================================================ # Generic .travis.yml file for running continuous integration on Travis-CI for # any ROS package. # # Available here: # - https://github.com/felixduvallet/ros-travis-integration # # This installs ROS on a clean Travis-CI virtual machine, creates a ROS # workspace, resolves all listed dependencies, and sets environment variables # (setup.bash). Then, it compiles the entire ROS workspace (ensuring there are # no compilation errors), and runs all the tests. If any of the compilation/test # phases fail, the build is marked as a failure. # # We handle two types of package dependencies specified in the package manifest: # - system dependencies that can be installed using `rosdep`, including other # ROS packages and system libraries. These dependencies must be known to # `rosdistro` and are installed using apt-get. # - package dependencies that must be checked out from source. These are handled by # `wstool`, and should be listed in a file named dependencies.rosinstall. # # There are envioronment variables you may want to change, such as ROS_DISTRO, # ROSINSTALL_FILE, and the CATKIN_OPTIONS file. See the README.md for more # information on these flags, and # https://docs.travis-ci.com/user/environment-variables/ for information about # Travis environment variables in general. # # Author: Felix Duvallet # NOTE: The build lifecycle on Travis.ci is something like this: # before_install # install # before_script # script # after_success or after_failure # after_script # OPTIONAL before_deploy # OPTIONAL deploy # OPTIONAL after_deploy ################################################################################ sudo: required cache: - apt # Build all valid Ubuntu/ROS combinations available on Travis VMs. language: generic matrix: include: - name: "focal noetic" dist: focal env: ROS_DISTRO=noetic - name: "Bionic melodic" dist: bionic env: ROS_DISTRO=melodic # Configuration variables. All variables are global now, but this can be used to # trigger a build matrix for different ROS distributions if desired. env: global: - ROS_CI_DESKTOP="`lsb_release -cs`" # e.g. [trusty|xenial|...] - CI_SOURCE_PATH=$(pwd) - ROSINSTALL_FILE=$CI_SOURCE_PATH/dependencies.rosinstall - ROS_PARALLEL_JOBS='-j8 -l6' # Set the python path manually to include /usr/-/python2.7/dist-packages # as this is where apt-get installs python packages. ################################################################################ # Install system dependencies, namely a very barebones ROS setup. before_install: - if [[ $ROS_DISTRO == "noetic" ]]; then export PYMODULE=python3; else export PYMODULE=python; fi; - export PYTHONPATH=$PYTHONPATH:/usr/lib/${PYMODULE}/dist-packages:/usr/local/lib/${PYMODULE}/dist-packages - sudo sh -c "echo \"deb http://packages.ros.org/ros/ubuntu $ROS_CI_DESKTOP main\" > /etc/apt/sources.list.d/ros-latest.list" - sudo apt-key adv --keyserver 'hkp://keyserver.ubuntu.com:80' --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654 - sudo apt-get update -qq - sudo apt-get install dpkg - sudo apt-get install -y ${PYMODULE}-catkin-pkg ${PYMODULE}-rosdep ${PYMODULE}-wstool ros-$ROS_DISTRO-desktop-full ros-$ROS_DISTRO-combined-robot-hw - source /opt/ros/$ROS_DISTRO/setup.bash - sudo apt install -y ros-$ROS_DISTRO-libfranka - if [[ $ROS_DISTRO == "melodic" ]]; then sudo apt install -y ros-$ROS_DISTRO-franka-ros; fi; - sudo apt install -y ${PYMODULE}-catkin-tools ${PYMODULE}-osrf-pycommon # Prepare rosdep to install dependencies. - sudo rosdep init - rosdep update --include-eol-distros # Support EOL distros. # Create a catkin workspace with the package under integration. install: - mkdir -p ~/catkin_ws/src - cd ~/catkin_ws/src - catkin_init_workspace # Create the devel/setup.bash (run catkin_make with an empty workspace) and # source it to set the path variables. - cd ~/catkin_ws - catkin build - source devel/setup.bash # Add the package under integration to the workspace using a symlink. - cd ~/catkin_ws/src - ln -s $CI_SOURCE_PATH . # Install all dependencies, using wstool first and rosdep second. # wstool looks for a ROSINSTALL_FILE defined in the environment variables. before_script: # source dependencies: install using wstool. - cd ~/catkin_ws/src - wstool init - if [[ -f $ROSINSTALL_FILE ]] ; then wstool merge $ROSINSTALL_FILE ; fi - wstool up - if [[ $ROS_DISTRO == "noetic" ]]; then git clone -b ${ROS_DISTRO}-devel https://github.com/frankaemika/franka_ros; git clone -b melodic-devel https://github.com/ros-planning/panda_moveit_config; fi; # package depdencies: install using rosdep. - cd ~/catkin_ws # - rosdep install -y --from-paths src --ignore-src --rosdistro $ROS_DISTRO # Compile and test (mark the build as failed if any step fails). If the # CATKIN_OPTIONS file exists, use it as an argument to catkin_make, for example # to blacklist certain packages. # # NOTE on testing: `catkin_make run_tests` will show the output of the tests # (gtest, nosetest, etc..) but always returns 0 (success) even if a test # fails. Running `catkin_test_results` aggregates all the results and returns # non-zero when a test fails (which notifies Travis the build failed). script: - source /opt/ros/$ROS_DISTRO/setup.bash - cd ~/catkin_ws # seems to need separate builds for succeeding when using franka_ros from source - if [[ $ROS_DISTRO == "noetic" ]]; then catkin build franka_msgs; source devel/setup.bash; catkin build franka_ros; fi; - source devel/setup.bash # Run the tests, ensuring the path is set correctly. - catkin build franka_ros_interface #--catkin-make-args run_tests && catkin_test_results ================================================ FILE: CMakeLists.txt ================================================ # toplevel CMakeLists.txt for a catkin workspace # catkin/cmake/toplevel.cmake cmake_minimum_required(VERSION 2.8.3) set(CATKIN_TOPLEVEL TRUE) # search for catkin within the workspace set(_cmd "catkin_find_pkg" "catkin" "${CMAKE_SOURCE_DIR}") execute_process(COMMAND ${_cmd} RESULT_VARIABLE _res OUTPUT_VARIABLE _out ERROR_VARIABLE _err OUTPUT_STRIP_TRAILING_WHITESPACE ERROR_STRIP_TRAILING_WHITESPACE ) if(NOT _res EQUAL 0 AND NOT _res EQUAL 2) # searching fot catkin resulted in an error string(REPLACE ";" " " _cmd_str "${_cmd}") message(FATAL_ERROR "Search for 'catkin' in workspace failed (${_cmd_str}): ${_err}") endif() # include catkin from workspace or via find_package() if(_res EQUAL 0) set(catkin_EXTRAS_DIR "${CMAKE_SOURCE_DIR}/${_out}/cmake") # include all.cmake without add_subdirectory to let it operate in same scope include(${catkin_EXTRAS_DIR}/all.cmake NO_POLICY_SCOPE) add_subdirectory("${_out}") else() # use either CMAKE_PREFIX_PATH explicitly passed to CMake as a command line argument # or CMAKE_PREFIX_PATH from the environment if(NOT DEFINED CMAKE_PREFIX_PATH) if(NOT "$ENV{CMAKE_PREFIX_PATH}" STREQUAL "") if(NOT WIN32) string(REPLACE ":" ";" CMAKE_PREFIX_PATH $ENV{CMAKE_PREFIX_PATH}) else() set(CMAKE_PREFIX_PATH $ENV{CMAKE_PREFIX_PATH}) endif() endif() endif() # list of catkin workspaces set(catkin_search_path "") foreach(path ${CMAKE_PREFIX_PATH}) if(EXISTS "${path}/.catkin") list(FIND catkin_search_path ${path} _index) if(_index EQUAL -1) list(APPEND catkin_search_path ${path}) endif() endif() endforeach() # search for catkin in all workspaces set(CATKIN_TOPLEVEL_FIND_PACKAGE TRUE) find_package(catkin QUIET NO_POLICY_SCOPE PATHS ${catkin_search_path} NO_DEFAULT_PATH NO_CMAKE_FIND_ROOT_PATH) unset(CATKIN_TOPLEVEL_FIND_PACKAGE) if(NOT catkin_FOUND) message(FATAL_ERROR "find_package(catkin) failed. catkin was neither found in the workspace nor in the CMAKE_PREFIX_PATH. One reason may be that no ROS setup.sh was sourced before.") endif() endif() catkin_workspace() ================================================ FILE: LICENSE ================================================ Apache License Version 2.0, January 2004 http://www.apache.org/licenses/ TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION 1. Definitions. "License" shall mean the terms and conditions for use, reproduction, and distribution as defined by Sections 1 through 9 of this document. "Licensor" shall mean the copyright owner or entity authorized by the copyright owner that is granting the License. "Legal Entity" shall mean the union of the acting entity and all other entities that control, are controlled by, or are under common control with that entity. For the purposes of this definition, "control" means (i) the power, direct or indirect, to cause the direction or management of such entity, whether by contract or otherwise, or (ii) ownership of fifty percent (50%) or more of the outstanding shares, or (iii) beneficial ownership of such entity. "You" (or "Your") shall mean an individual or Legal Entity exercising permissions granted by this License. "Source" form shall mean the preferred form for making modifications, including but not limited to software source code, documentation source, and configuration files. "Object" form shall mean any form resulting from mechanical transformation or translation of a Source form, including but not limited to compiled object code, generated documentation, and conversions to other media types. "Work" shall mean the work of authorship, whether in Source or Object form, made available under the License, as indicated by a copyright notice that is included in or attached to the work (an example is provided in the Appendix below). "Derivative Works" shall mean any work, whether in Source or Object form, that is based on (or derived from) the Work and for which the editorial revisions, annotations, elaborations, or other modifications represent, as a whole, an original work of authorship. For the purposes of this License, Derivative Works shall not include works that remain separable from, or merely link (or bind by name) to the interfaces of, the Work and Derivative Works thereof. "Contribution" shall mean any work of authorship, including the original version of the Work and any modifications or additions to that Work or Derivative Works thereof, that is intentionally submitted to Licensor for inclusion in the Work by the copyright owner or by an individual or Legal Entity authorized to submit on behalf of the copyright owner. For the purposes of this definition, "submitted" means any form of electronic, verbal, or written communication sent to the Licensor or its representatives, including but not limited to communication on electronic mailing lists, source code control systems, and issue tracking systems that are managed by, or on behalf of, the Licensor for the purpose of discussing and improving the Work, but excluding communication that is conspicuously marked or otherwise designated in writing by the copyright owner as "Not a Contribution." "Contributor" shall mean Licensor and any individual or Legal Entity on behalf of whom a Contribution has been received by Licensor and subsequently incorporated within the Work. 2. Grant of Copyright License. Subject to the terms and conditions of this License, each Contributor hereby grants to You a perpetual, worldwide, non-exclusive, no-charge, royalty-free, irrevocable copyright license to reproduce, prepare Derivative Works of, publicly display, publicly perform, sublicense, and distribute the Work and such Derivative Works in Source or Object form. 3. Grant of Patent License. Subject to the terms and conditions of this License, each Contributor hereby grants to You a perpetual, worldwide, non-exclusive, no-charge, royalty-free, irrevocable (except as stated in this section) patent license to make, have made, use, offer to sell, sell, import, and otherwise transfer the Work, where such license applies only to those patent claims licensable by such Contributor that are necessarily infringed by their Contribution(s) alone or by combination of their Contribution(s) with the Work to which such Contribution(s) was submitted. If You institute patent litigation against any entity (including a cross-claim or counterclaim in a lawsuit) alleging that the Work or a Contribution incorporated within the Work constitutes direct or contributory patent infringement, then any patent licenses granted to You under this License for that Work shall terminate as of the date such litigation is filed. 4. Redistribution. You may reproduce and distribute copies of the Work or Derivative Works thereof in any medium, with or without modifications, and in Source or Object form, provided that You meet the following conditions: (a) You must give any other recipients of the Work or Derivative Works a copy of this License; and (b) You must cause any modified files to carry prominent notices stating that You changed the files; and (c) You must retain, in the Source form of any Derivative Works that You distribute, all copyright, patent, trademark, and attribution notices from the Source form of the Work, excluding those notices that do not pertain to any part of the Derivative Works; and (d) If the Work includes a "NOTICE" text file as part of its distribution, then any Derivative Works that You distribute must include a readable copy of the attribution notices contained within such NOTICE file, excluding those notices that do not pertain to any part of the Derivative Works, in at least one of the following places: within a NOTICE text file distributed as part of the Derivative Works; within the Source form or documentation, if provided along with the Derivative Works; or, within a display generated by the Derivative Works, if and wherever such third-party notices normally appear. The contents of the NOTICE file are for informational purposes only and do not modify the License. You may add Your own attribution notices within Derivative Works that You distribute, alongside or as an addendum to the NOTICE text from the Work, provided that such additional attribution notices cannot be construed as modifying the License. You may add Your own copyright statement to Your modifications and may provide additional or different license terms and conditions for use, reproduction, or distribution of Your modifications, or for any such Derivative Works as a whole, provided Your use, reproduction, and distribution of the Work otherwise complies with the conditions stated in this License. 5. Submission of Contributions. Unless You explicitly state otherwise, any Contribution intentionally submitted for inclusion in the Work by You to the Licensor shall be under the terms and conditions of this License, without any additional terms or conditions. Notwithstanding the above, nothing herein shall supersede or modify the terms of any separate license agreement you may have executed with Licensor regarding such Contributions. 6. Trademarks. This License does not grant permission to use the trade names, trademarks, service marks, or product names of the Licensor, except as required for reasonable and customary use in describing the origin of the Work and reproducing the content of the NOTICE file. 7. Disclaimer of Warranty. Unless required by applicable law or agreed to in writing, Licensor provides the Work (and each Contributor provides its Contributions) on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied, including, without limitation, any warranties or conditions of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A PARTICULAR PURPOSE. You are solely responsible for determining the appropriateness of using or redistributing the Work and assume any risks associated with Your exercise of permissions under this License. 8. Limitation of Liability. In no event and under no legal theory, whether in tort (including negligence), contract, or otherwise, unless required by applicable law (such as deliberate and grossly negligent acts) or agreed to in writing, shall any Contributor be liable to You for damages, including any direct, indirect, special, incidental, or consequential damages of any character arising as a result of this License or out of the use or inability to use the Work (including but not limited to damages for loss of goodwill, work stoppage, computer failure or malfunction, or any and all other commercial damages or losses), even if such Contributor has been advised of the possibility of such damages. 9. Accepting Warranty or Additional Liability. While redistributing the Work or Derivative Works thereof, You may choose to offer, and charge a fee for, acceptance of support, warranty, indemnity, or other liability obligations and/or rights consistent with this License. However, in accepting such obligations, You may act only on Your own behalf and on Your sole responsibility, not on behalf of any other Contributor, and only if You agree to indemnify, defend, and hold each Contributor harmless for any liability incurred by, or claims asserted against, such Contributor by reason of your accepting any such warranty or additional liability. END OF TERMS AND CONDITIONS APPENDIX: How to apply the Apache License to your work. To apply the Apache License to your work, attach the following boilerplate notice, with the fields enclosed by brackets "[]" replaced with your own identifying information. (Don't include the brackets!) The text should be enclosed in the appropriate comment syntax for the file format. We also recommend that a file or class name and description of purpose be included on the same "printed page" as the copyright notice for easier identification within third-party archives. Copyright 2019-2021 Saif Sidhik Licensed under the Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License. You may obtain a copy of the License at http://www.apache.org/licenses/LICENSE-2.0 Unless required by applicable law or agreed to in writing, software distributed under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the License for the specific language governing permissions and limitations under the License. ================================================ FILE: NOTICE ================================================ franka_ros_interface package Copyright 2019-2021 Saif Sidhik Licensed under the Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License. You may obtain a copy of the License at http://www.apache.org/licenses/LICENSE-2.0 Unless required by applicable law or agreed to in writing, software distributed under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the License for the specific language governing permissions and limitations under the License. ================================================ FILE: README.md ================================================ # Franka ROS Interface [![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/) [![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) [![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) A ROS interface library for the Franka Emika Panda robot (**_real and [simulated][ps-repo]_**), extending the [franka-ros][franka-ros] library to expose more information about the robot, and providing low-level control of the robot using ROS and [Python API][fri-doc]. Franka ROS Interface provides utilites for controlling and managing the Franka Emika Panda robot. Contains exposed customisable controllers for the robot (joint position, velocity, torque), interfaces for the gripper, controller manager, coordinate frames interface, etc. Also provides utilities to control the robot using 'MoveIt!' and ROS Trajectory Action & ActionClient. This package also provides almost complete sim-to-real / real-to-sim transfer of code with the [Panda Simulator][ps-repo] package. Documentation Page: **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.) A more unified ROS Python interface built over this package is available at [PandaRobot](https://github.com/justagist/panda_robot), which provides a more intuitive interface class that combines the different API classes in this package. Simple demos are also available. ## Features - Low-level *controllers* (joint position, velocity, torque, impedance) available that can be controlled through ROS topics and Python API (including position control for gripper). - Real-time *robot state* (end-effector state, joint state, controller state, etc.) available through ROS topics and Python API. - Python API for managing controllers, coordinate frames, collision behaviour, controlling and monitoring the gripper. - Python API classes and utility functions to control the robot using MoveIt! and ROS Trajectory Action Service. - The [panda_simulator][ps-repo] package (which is Gazebo-based simulator for the robot) can also be controlled using this package (ROS and Python interface), providing almost complete sim-to-real transfer of code. **Demo Using [PandaRobot API](https://github.com/justagist/panda_robot) and [Panda Simulator][ps-repo]** ![vid](assets/panda_robot_demo.gif) Watch video [here](https://youtu.be/4bEVysUIvOY) ![vid](assets/panda_simulator.gif) Watch video [here](https://www.youtube.com/watch?v=NdSbXC0r7tU) ![vid](assets/ts_demo.gif) Watch video [here](https://youtu.be/a_HEmYzqEnk) ## Installation This 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) **NOTE:** Tested on: | ROS Version | Python Version | Franka ROS Branch | |-------------|----------------|-------------------------------------------------------------------------------| | Melodic | 2.7+ | [melodic-devel](https://github.com/frankaemika/franka_ros/tree/melodic-devel) | | Noetic | 3.6+ | [noetic-devel](https://github.com/frankaemika/franka_ros/tree/noetic-devel) | ### Dependencies - ROS Melodic / Noetic (preferably the 'desktop-full' version) - *libfranka (required version >= 0.8.0)* (`sudo apt install ros-$ROS_DISTRO-libfranka` or [install from source][libfranka-doc]). *Use the release version if building from source for reliable build.* - *franka-ros* v0.7.1 (`sudo apt install ros-$ROS_DISTRO-franka-ros` or [install from source][franka-ros]). *Make sure to use the appropriate branch of franka_ros (`melodic-devel` or `noetic-devel`) depending on your ROS version.* - *panda_moveit_config* (`sudo apt install ros-$ROS_DISTRO-panda-moveit-config` or [install from source](https://github.com/ros-planning/panda_moveit_config) (the 'melodic-devel' branch also works for ROS Noetic)) - [*franka_panda_description*][fpd-repo] (Clone this repository to the `src` folder of your workspace. See [Related Packages](#related-packages) section for information about package). **NOTE**: Installing this package is optional, but recommended. If you do not want to use the *franka_panda_description* package, make sure you modify the `franka_interface/launch/interface.launch` file and replace all occurences of `franka_panda_description` with `franka_description` (two occurences). Once the above dependencies are installed, the package can be installed using catkin: ```sh cd git clone -b v0.7.1-dev https://github.com/justagist/franka_ros_interface src/franka_ros_interface catkin build # or catkin_make (catkin build is recommended) source devel/setup.bash ``` *Note: Python code in this package is written to be compatible with both Python 2 and 3, so make sure you have the Python `future` module installed (`pip install future` (or `pip3 install ..`)).* After building the package (this is not required if using with simulated robot): - Copy/move the *franka.sh* file to the root of the catkin_ws `$ cp src/franka_ros_interface/franka.sh ./` - Change the values in the copied file (described in the file). ## Usage **NOTE: For using this package with [Panda Simulator][ps-repo], the following sections are not required; all the required "driver" nodes are started along with the simulation launch file, and the Franka ROS Interface API (as well as [PandaRobot](https://github.com/justagist/panda_robot) API) can be directly used. Follow instructions in the [demos](https://github.com/justagist/panda_simulator#demos) section in the Panda Simulator package. See their corresponding [source files](https://github.com/justagist/panda_simulator/tree/melodic-devel/panda_simulator_examples/scripts) for usage examples.** ### The *franka.sh* environments Once the values are correctly modified in the `franka.sh` file, different environments can be set for controlling the robot by sourcing this file. - For instance, running `./franka.sh master` would start an environment assuming that the computer is directly connected to the robot (requires Real-Time kernel set up as described in the [FCI documentation][libfranka-doc]). - On the other hand, `./franka.sh remote` would start an environment assuming that the robot is not connected directly to the computer, but to another computer in the network (whose IP must be specified in *franka.sh*). This way, if the 'master' is connected to the robot and running the driver node (see below), the 'remote' can control the robot (**_no need for Real Time kernel!_**) as long as they are in the same network. - ~Simulation environment can be started by running `./franka.sh sim` (only required when using [panda_simulator][ps-repo] package).~ *Not required anymore* More information regarding the usage of `franka.sh` can be found within the [franka.sh file](franka.sh). ### Starting the Franka ROS Interface 'Driver' The 'driver' node can be started by running (can only be used if run in 'master' environment - see [Environments](#the-frankash-environments) section above): ```sh roslaunch franka_interface interface.launch # (use argument load_gripper:=false for starting without gripper) ``` Available keyword arguments for launch file: - `load_gripper`: start driver node with the Franka gripper (default: `true`). - `start_controllers`: load the available controllers to the controller manager (default: `true`). - `start_moveit`: start moveit server along with the driver node (default: `true`). - `load_demo_planning_scene`: loads a default planning scene for MoveIt planning with simple objects for collision avoidance (default: `true`). See [create_demo_planning_scene.py](franka_moveit/scripts/create_demo_planning_scene.py). This starts the robot controllers and drivers to expose a variety of ROS topics and services for communicating with and controlling the robot. ### Controlling and Monitoring the Robot Once the 'driver' is running, the robot can be controlled from another terminal by running in 'master' environment (if running in the same machine as 'driver'), or 'remote' environment (if using a different connected computer). The robot can then be controlled and monitored using ROS topics and services (see below to find out about some of the available topics and services), or using the provided [Python API][fri-doc] (also see [*PandaRobot*](https://github.com/justagist/panda_robot)). Example of the robot working with MoveIt can be found by running `roslaunch franka_moveit demo_moveit.launch`. Basic usage of the Python API is shown in the [`demo_interface.py` example file](franka_interface/tests/demo_interface.py). An example of controlling the robot joints with keyboard using direct position control is shown in [`demo_joint_positions_keyboard.py`](https://github.com/justagist/franka_ros_interface/blob/master/franka_interface/tests/demo_joint_positions_keyboard.py) (read the notes in the file before running the demo. start this demo by running `rosrun franka_interface joint_position_keyboard.py`). See [documentation][fri-doc] for all available methods and functionalities. See the [tests](https://github.com/justagist/franka_ros_interface/tree/master/franka_interface/tests) and [scripts](https://github.com/justagist/franka_ros_interface/tree/master/franka_interface/scripts) directories in `franka_interface` for few more usage examples. **More usage examples can be found in the [PandaRobot](https://github.com/justagist/panda_robot) package.** #### Some useful ROS topics ##### Published Topics | ROS Topic | Data | | ------ | ------ | | */franka_ros_interface/custom_franka_state_controller/robot_state* | gravity, coriolis, jacobian, cartesian velocity, etc. | | */franka_ros_interface/custom_franka_state_controller/tip_state* | end-effector pose, wrench, etc. | | */franka_ros_interface/joint_states* | joint positions, velocities, efforts | | */franka_ros_interface/franka_gripper/joint_states* | joint positions, velocities, efforts of gripper joints | ##### Subscribed Topics | ROS Topic | Data | | ------ | ------ | | */franka_ros_interface/motion_controller/arm/joint_commands* | command the robot using the currently active controller | | */franka_ros_interface/franka_gripper/[move/grasp/stop/homing]* | (action msg) command the joints of the gripper | Other topics for changing the controller gains (also dynamically configurable), command timeout, etc. are also available. #### ROS Services Controller manager service can be used to switch between all available controllers (joint position, velocity, effort). Gripper joints can be controlled using the ROS ActionClient. Other services for changing coordinate frames, adding gripper load configuration, etc. are also available. #### Python API Most of the above services and topics are wrapped using simple Python classes or utility functions, providing more control and simplicity. This includes direct control of the robot and gripper using the provided custom low-level controllers, MoveIt, and JointTrajectoryAction. Refer README files in individual subpackages. [Documentation][fri-doc] More usage examples in the [PandaRobot](https://github.com/justagist/panda_robot) package. ## Related Packages - [*panda_simulator*][ps-repo] : A Gazebo simulator for the Franka Emika Panda robot with ROS interface, providing exposed controllers and real-time robot state feedback similar to the real robot when using the *franka_ros_interface* package. Provides almost complete real-to-sim transfer of code. - [*PandaRobot*](https://github.com/justagist/panda_robot) : Python interface providing higher-level control of the robot integrated with its gripper control, controller manager, coordinate frames manager, etc. with safety checks and other helper utilities. It also provides the kinematics and dynamics of the robot using the [KDL library](http://wiki.ros.org/kdl). It is built over Franka ROS Interface and provides a more intuitive and unified single-class interface. - [*franka_panda_description*][fpd-repo] : Robot description package modified from [*franka_ros*][franka-ros] package to include dynamics parameters for the robot arm (as estimated in [this paper](https://hal.inria.fr/hal-02265293/document)). Also includes transmission and control definitions required for the [*panda_simulator*][ps-repo] package. [ps-repo]: [fri-repo]: [fpd-repo]: [fri-doc]: [libfranka-doc]: [franka-ros]: ## License [![License](https://img.shields.io/badge/License-Apache%202.0-blue.svg)](https://opensource.org/licenses/Apache-2.0) Copyright (c) 2019-2021, Saif Sidhik If 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). ================================================ FILE: cmake/ClangTools.cmake ================================================ include(CMakeParseArguments) find_program(CLANG_FORMAT_PROG clang-format-5.0 DOC "'clang-format' executable") if(CLANG_FORMAT_PROG AND NOT TARGET format) add_custom_target(format) add_custom_target(check-format) endif() find_program(CLANG_TIDY_PROG clang-tidy-5.0 DOC "'clang-tidy' executable") if(CLANG_TIDY_PROG AND NOT TARGET tidy) if(NOT CMAKE_EXPORT_COMPILE_COMMANDS) message(WARNING "Invoke Catkin/CMake with '-DCMAKE_EXPORT_COMPILE_COMMANDS=ON' to generate compilation database for 'clang-tidy'.") endif() add_custom_target(tidy) add_custom_target(check-tidy) endif() function(add_format_target _target) if(NOT CLANG_FORMAT_PROG) return() endif() cmake_parse_arguments(ARG "" "" "FILES" ${ARGN}) add_custom_target(format-${_target} COMMAND ${CLANG_FORMAT_PROG} -i ${ARG_FILES} WORKING_DIRECTORY ${CMAKE_CURRENT_LIST_DIR}/.. COMMENT "Formatting ${_target} source code with clang-format" VERBATIM ) add_dependencies(format format-${_target}) add_custom_target(check-format-${_target} COMMAND scripts/format-check.sh ${CLANG_FORMAT_PROG} -output-replacements-xml ${ARG_FILES} WORKING_DIRECTORY ${CMAKE_CURRENT_LIST_DIR}/.. COMMENT "Checking ${_target} code formatting with clang-format" VERBATIM ) add_dependencies(check-format check-format-${_target}) endfunction() function(add_tidy_target _target) if(NOT CLANG_TIDY_PROG) return() endif() cmake_parse_arguments(ARG "" "" "FILES;DEPENDS" ${ARGN}) add_custom_target(tidy-${_target} COMMAND ${CLANG_TIDY_PROG} -fix -p=${CMAKE_BINARY_DIR} ${ARG_FILES} WORKING_DIRECTORY ${CMAKE_CURRENT_LIST_DIR}/.. DEPENDS ${ARG_DEPENDS} COMMENT "Running clang-tidy for ${_target}" VERBATIM ) add_dependencies(tidy tidy-${_target}) add_custom_target(check-tidy-${_target} COMMAND scripts/fail-on-output.sh ${CLANG_TIDY_PROG} -p=${CMAKE_BINARY_DIR} ${ARG_FILES} WORKING_DIRECTORY ${CMAKE_CURRENT_LIST_DIR}/.. DEPENDS ${ARG_DEPENDS} COMMENT "Running clang-tidy for ${_target}" VERBATIM ) add_dependencies(check-tidy check-tidy-${_target}) endfunction() ================================================ FILE: franka.sh ================================================ #!/bin/bash # Copyright (c) 2019-2021, Saif Sidhik # Copyright (c) 2013-2016, Rethink Robotics # All rights reserved. # This is a convenient script which will set up your ROS environment and # should be executed with every new instance of a shell in which you plan on # working with Franka ROS Interface or PandaRobot. # This file is to be used in the *root* of your catkin workspace. # ****** IMPORTANT: Read "USAGE EXPLAINED" section below before running this script. ****** # Run as './franka.sh ' # ------ ARGUMENTS: # -- arg1 (required): select the environment for Franka ROS Interface. The available options are: # - '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). # - '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. # - 'sim': start in simulation environment (to be used with panda_simulator package only). [Not necessary] # -- arg2 (optional): # - (valid only if is 'slave'/'remote'): IP address of ROS Master # eg: # ./franka.sh master # (or) # ./franka.sh remote [optional IP address of master] ## ========================================= # ## === EDIT FOLLOWING VALUES AS REQUIRED === # ## ========================================= # # ----- EDIT THIS TO MATCH THE IP OF THE FRANKA ROBOT CONTROLLER IN THE NETWORK FRANKA_ROBOT_IP="FRANKA_ROBOT_IP.local" # ----- EDIT THIS TO MATCH YOUR IP IN THE NETWORK your_ip="" # ----- EDIT THIS TO MATCH THE IP OF THE MAIN ROS MASTER PC (CONNECTED TO THE FRANKA CONTROLLER), # ----- ON WHICH THE Franka ROS Interface 'driver' NODES WILL BE RUNNING (SEE "USAGE EXPLAINED" BELOW). # ----- THIS VALUE IS NEEDED ONLY IF YOU WILL USE THE 'remote' ENVIRONMENT ON THIS MACHINE. ROS_MASTER_IP="" ros_version="melodic" ## ========================================= # ## ========== USAGE EXPLAINED ============== # ## ========================================= # # 1. If there is only one computer controlling the robot, there is no need # to fill the $ROS_MASTER_IP variable in the field above. However, this # machine should have RT kernel set up correctly and should be able to # connect to the robot using libfranka. # For using Franka ROS Interface in this case: # a) Move this file to the root of the catkin ws, and fill in the # values for $FRANKA_ROBOT_IP and $your_ip above. # b) run `./franka.sh master` # c) Start the driver node (see Usage in README) # d) Once the driver node is running correctly, on a different # terminal, run `./franka master` again from the catkin_ws root to # connect to the same ROS master as the driver node. # e) You should now be able to communicate with the robot using # Franka ROS Interface from this terminal session. Use ROS # topics/services etc. or Franka ROS Interface Python API or scripts # to control and monitor the robot. (test by running `rostopic list`) # f) For every new terminal, make sure to run `./franka.sh master` first. # 2. If you want to use multiple machines for controlling the robot, all # machines should be in the same network as the robot controller. # AT LEAST ONE MACHINE should have RT kernel set up and should be able to # properly connect to the robot using libfranka. ONE of these machines # should run the driver node as explained in steps 1.a - 1.c above. # Follow steps 1.d - 1.f if Franka ROS Interface is needed againg on this # RT machine (which is running the driver). # # NOTE: ONLY ONE MACHINE IN THE NETWORK SHOULD BE RUNNING THE DRIVER NODE. # # Other computers in the network need not have RT kernels, but should have # correctly installed lifranka, franka-ros, and Franka ROS Interface. For # controlling the robot from these machines: # a) Move this file to the root of the catkin_ws of the remote machines, # and fill in the values for $FRANKA_ROBOT_IP, $your_ip, and # $ROS_MASTER_IP above. THE $ROS_MASTER_IP should correspond to the # IP address of the RT machine running the 'driver' node. # b) Then run `./franka.sh remote`. This should connect the current # session to the ROS master running the driver node (to test if the # connection is correct, run `rostopic list` to see if the published # and subscribed topics from Franka ROS Interface driver node is # accessible from the remote computer). # c) As long as the driver node is running on the RT machine, you should # now be able to communicate with the robot using Franka ROS Interface # from this terminal session. Use ROS topics/services etc. or Franka # ROS Interface Python API or scripts to control and monitor the robot. # d) For every new terminal in the remote machines, make sure to run # `./franka.sh remote` first. ## ========================================= # ## ========================================= # ## === DO NOT EDIT BELOW THIS LINE ========= # if [ "${1}" == "sim" ]; then your_ip="127.0.0.1" fi if [ "${your_ip}" == "" ]; then echo -e "\n\t\033[01;31m\033[1mError:\033[00m [franka.sh] \033[01;33m\033[4myour_ip\033[00m variable has to be set in 'franka.sh'\n" exit 1 fi if [ "${1}" == "master" ] || [[ "${1}" == "local" ]]; then if [ "${FRANKA_ROBOT_IP}" == "FRANKA_ROBOT_IP.local" ]; then echo -e "\n\t\033[01;31m\033[1mError:\033[00m [franka.sh] \033[01;33m\033[4mFRANKA_ROBOT_IP\033[00m variable has to be set to match the IP address of the Franka Robot Controller\n" exit 1 fi ROS_MASTER_IP="$your_ip" IS_MASTER=true elif [ "${1}" == "slave" ] || [[ "${1}" == "remote" ]]; then if [[ $# -ge 2 ]]; then ROS_MASTER_IP="$2" fi IS_MASTER=false elif [ "${1}" == "sim" ]; then ROS_MASTER_IP="$your_ip" IS_MASTER="true" FRANKA_ROBOT_IP="sim" else echo -e "\n\t\033[01;31m\033[1mError:\033[00m [franka.sh] Atleast one argument has to be passed Usage: ./franka_sdk ------ ARGUMENTS: -- arg1: - 'master' / 'local' (No ): The local machine will be the ros master. Run this on the main master pc (connected to the franka controller). - 'slave' / 'remote': If IP address is provided as , 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 - 'sim': Franka robot simulation environment will be used. Real robot will not be controlled. -- arg2: - (valid only if is 'slave'/'remote'): IP address of ROS Master " exit 1 fi tf=$(mktemp) trap "rm -f -- '${tf}'" EXIT topdir=$(basename $(readlink -f $(dirname ${BASH_SOURCE[0]}))) cat <<-EOF > ${tf} [ -s "\${HOME}"/.bashrc ] && source "\${HOME}"/.bashrc [ -s "\${HOME}"/.bash_profile ] && source "\${HOME}"/.bash_profile # verify this script is moved out of franka_sdk folder if [[ -e "${topdir}/franka_rospy_sdk/package.xml" ]]; then echo -ne "EXITING - This script must be moved from the franka_rospy_sdk folder \ to the root of your catkin workspace.\n" exit 1 fi # check for ros installation if [ ! -d "/opt/ros" ] || [ ! "$(ls -A /opt/ros)" ]; then echo "EXITING - No ROS installation found in /opt/ros." echo "Is ROS installed?" exit 1 fi # verify specified ros version is installed ros_setup="/opt/ros/${ros_version}" if [ ! -d "\${ros_setup}" ]; then echo -ne "EXITING - Failed to find ROS \${ros_version} installation \ in \${ros_setup}.\n" exit 1 fi # verify the ros setup.sh file exists if [ ! -s "\${ros_setup}"/setup.sh ]; then echo -ne "EXITING - Failed to find the ROS environment script: \ "\${ros_setup}"/setup.sh.\n" exit 1 fi # verify the user is running this script in the root of the catkin # workspace and that the workspace has been compiled. if [ ! -s "devel/setup.bash" ]; then echo -ne "EXITING - \n1) Please verify that this script is being run \ in the root of your catkin workspace.\n2) Please verify that your workspace \ has been built (source /opt/ros/\${ros_version}/setup.sh; catkin_make).\n\ 3) Run this script again upon completion of your workspace build.\n" exit 1 fi export FRANKA_ROBOT_IP=$FRANKA_ROBOT_IP [ -n "${your_ip}" ] && export ROS_IP="${your_ip}" [ -n "${ROS_MASTER_IP}" ] && \ export ROS_MASTER_URI="http://${ROS_MASTER_IP}:11311" # source the catkin setup bash script source devel/setup.bash if [[ $IS_MASTER == true ]]; then echo -e "\nROBOT: $FRANKA_ROBOT_IP\n" else echo -e "\nROS MASTER: $ROS_MASTER_IP\n" fi # setup the bash prompt export __ROS_PROMPT=\${__ROS_PROMPT:-0} [ \${__ROS_PROMPT} -eq 0 -a -n "\${PROMPT_COMMAND}" ] && \ export __ORIG_PROMPT_COMMAND=\${PROMPT_COMMAND} __ros_prompt () { if [ -n "\${__ORIG_PROMPT_COMMAND}" ]; then eval \${__ORIG_PROMPT_COMMAND} fi if ! echo \${PS1} | grep '\[franka' &>/dev/null; then if [[ $your_ip == "localhost" ]]; then export PS1="\[\033[00;33m\][franka - Robot@\ ${FRANKA_ROBOT_IP}]\[\033[00m\] \${PS1}" elif [[ $IS_MASTER == true ]]; then export PS1="\[\033[00;33m\][franka - Robot@\ ${FRANKA_ROBOT_IP}]\[\033[00m\] \${PS1}" else export PS1="\[\033[00;33m\][franka - Master@\ ${ROS_MASTER_IP}]\[\033[00m\] \${PS1}" fi fi } if [ "\${TERM}" != "dumb" ]; then export PROMPT_COMMAND=__ros_prompt __ROS_PROMPT=1 elif ! echo \${PS1} | grep '\[franka' &>/dev/null; then if [[ $your_ip == "localhost" ]]; then export PS1="\[\033[00;33m\][franka - Robot@\ ${FRANKA_ROBOT_IP}]\[\033[00m\] \${PS1}" elif [[ $IS_MASTER == true ]]; then export PS1="\[\033[00;33m\][franka - Robot@\ ${FRANKA_ROBOT_IP}]\[\033[00m\] \${PS1}" else export PS1="\[\033[00;33m\][franka - Master@\ ${ROS_MASTER_IP}]\[\033[00m\] \${PS1}" fi fi EOF ${SHELL} --rcfile ${tf} rm -f -- "${tf}" trap - EXIT ================================================ FILE: franka_common/franka_core_msgs/CMakeLists.txt ================================================ cmake_minimum_required(VERSION 2.8.3) project(franka_core_msgs) find_package(catkin REQUIRED COMPONENTS message_generation actionlib_msgs std_msgs franka_msgs geometry_msgs sensor_msgs control_msgs ) add_message_files( DIRECTORY msg FILES JointCommand.msg RobotState.msg EndPointState.msg JointLimits.msg JointControllerStates.msg ) # add_service_files( DIRECTORY srv # FILES # SolvePositionIK.srv # SolvePositionFK.srv # IOComponentCommandSrv.srv # ) # add_action_files(DIRECTORY action FILES CalibrationCommand.action) ## Build generate_messages(DEPENDENCIES std_msgs geometry_msgs control_msgs sensor_msgs actionlib_msgs franka_msgs) catkin_package(CATKIN_DEPENDS message_runtime std_msgs geometry_msgs control_msgs sensor_msgs franka_msgs actionlib_msgs) ## Install ## Testing ================================================ FILE: franka_common/franka_core_msgs/msg/EndPointState.msg ================================================ std_msgs/Header header float64[16] O_T_EE # Measured end effector pose in base frame # ----- Moved ee velocity to robot state, because it is being computed using J*dq. EE vel is not being provided natively #float64[6] O_dP_EE_c # Last commanded end effector twist in base frame. #float64[6] O_dP_EE_d # Desired end effector twist in base frame. #float64[6] O_ddP_EE_c # Last commanded end effector acceleration in base frame. geometry_msgs/WrenchStamped O_F_ext_hat_K # Estimated external wrench (force, torque) acting on stiffness frame, expressed relative to the base frame geometry_msgs/WrenchStamped K_F_ext_hat_K # Estimated external wrench (force, torque) acting on stiffness frame, expressed relative to the stiffness frame (ee_frame by default) ================================================ FILE: franka_common/franka_core_msgs/msg/JointCommand.msg ================================================ Header header int32 mode # Mode in which to command arm string[] names # Joint names order for command # Fields of commands indexed according to the Joint names vector. # Command fields required for a desired mode are listed in the comments float64[] position # (radians) Required for POSITION_MODE and IMPEDANCE_MODE float64[] velocity # (radians/sec) Required for VELOCITY_MODE and IMPEDANCE_MODE float64[] acceleration # (radians/sec^2) Required for float64[] effort # (newton-meters) Required for TORQUE_MODE # Modes available to command arm int32 POSITION_MODE=1 int32 VELOCITY_MODE=2 int32 TORQUE_MODE=3 int32 IMPEDANCE_MODE=4 ================================================ FILE: franka_common/franka_core_msgs/msg/JointControllerStates.msg ================================================ Header header string controller_name string[] names # Joint names order for command control_msgs/JointControllerState[] joint_controller_states ================================================ FILE: franka_common/franka_core_msgs/msg/JointLimits.msg ================================================ # names of the joints string[] joint_names # lower bound on the angular position in radians float64[] position_lower # upper bound on the angular position in radians float64[] position_upper # symmetric maximum joint velocity in radians/second float64[] velocity # symmetric maximum joint acceleration in radians/second^2 float64[] accel # symmetric maximum joint torque in Newton-meters float64[] effort ================================================ FILE: franka_common/franka_core_msgs/msg/RobotState.msg ================================================ std_msgs/Header header float64[6] cartesian_collision float64[6] cartesian_contact float64[6] O_dP_EE # EE vel computed as J*dq # float64[7] q # joint position, velocity, and effort in joint_states topic # float64[7] dq float64[7] q_d float64[7] dq_d # float64[7] tau_J float64[7] dtau_J # torque derivative float64[7] tau_J_d # desired joint torque # float64[6] K_F_ext_hat_K # float64[2] elbow # float64[2] elbow_d float64[7] joint_collision float64[7] joint_contact # float64[6] O_F_ext_hat_K # in tip state float64[7] tau_ext_hat_filtered # filtered external torque float64[3] F_x_Cee # Configured center of mass of the end effector load with respect to flange frame. float64[3] F_x_Cload # Configured center of mass of the external load with respect to flange frame. float64[3] F_x_Ctotal # Combined center of mass of the end effector load and the external load with respect to flange frame. float64[9] I_ee # Configured rotational inertia matrix of the end effector load with respect to center of mass. float64[9] I_load float64[9] I_total float64 m_ee # configured mass of end-effector float64 m_load # configured mass of external load float64 m_total float64[7] gravity float64[7] coriolis float64[49] mass_matrix # mass matrix of end-effector wrt base frame # Vectorized 7x7, column-major float64[42] O_Jac_EE # zero jacobian of end-effector frame. Vectorized 6x7 Jacobian, column-major # float64[16] O_T_EE # ----- moved to endpointstate msg # Vectorized 4x4, column-major float64[16] O_T_EE_d # Last desired end effector pose of motion generation in base frame. # Vectorized 4x4, column-major float64[16] F_T_EE # End effector frame pose in flange frame # Vectorized 4x4, column-major float64[16] F_T_NE # Nominal End effector frame pose in flange frame (fixed in Desk) # Vectorized 4x4, column-major float64[16] NE_T_EE # End effector frame pose in Nominal End effector frame # Vectorized 4x4, column-major float64[16] EE_T_K # Stiffness frame pose in end effector frame # Vectorized 4x4, column-major float64 time uint8 ROBOT_MODE_OTHER=0 uint8 ROBOT_MODE_IDLE=1 uint8 ROBOT_MODE_MOVE=2 uint8 ROBOT_MODE_GUIDING=3 uint8 ROBOT_MODE_REFLEX=4 uint8 ROBOT_MODE_USER_STOPPED=5 uint8 ROBOT_MODE_AUTOMATIC_ERROR_RECOVERY=6 uint8 robot_mode franka_msgs/Errors current_errors franka_msgs/Errors last_motion_errors ================================================ FILE: franka_common/franka_core_msgs/package.xml ================================================ franka_core_msgs 0.7.1 Messages and Services required for communication with the Franka robot when using franka_ros_interface and panda_simulator Saif Sidhik Apache 2.0 Saif Sidhik catkin message_generation std_msgs geometry_msgs sensor_msgs control_msgs franka_msgs actionlib_msgs message_runtime std_msgs franka_msgs geometry_msgs control_msgs sensor_msgs actionlib_msgs ================================================ FILE: franka_interface/CMakeLists.txt ================================================ cmake_minimum_required(VERSION 2.8.3) project(franka_interface) set(CMAKE_BUILD_TYPE Release) set(CMAKE_CXX_STANDARD 14) set(CMAKE_CXX_STANDARD_REQUIRED ON) find_package(catkin REQUIRED COMPONENTS controller_interface dynamic_reconfigure franka_hw franka_control controller_manager franka_core_msgs geometry_msgs hardware_interface pluginlib realtime_tools roscpp rospy ) find_package(Eigen3 REQUIRED) find_package(Franka 0.5.0 REQUIRED) catkin_package( LIBRARIES custom_franka_state_controller CATKIN_DEPENDS controller_interface franka_msgs dynamic_reconfigure franka_hw franka_control franka_core_msgs geometry_msgs hardware_interface pluginlib realtime_tools roscpp DEPENDS Franka ) add_library(custom_franka_state_controller src/robot_state_controller.cpp ) add_dependencies(custom_franka_state_controller ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS} ) target_link_libraries(custom_franka_state_controller PUBLIC ${Franka_LIBRARIES} ${catkin_LIBRARIES} ) target_include_directories(custom_franka_state_controller SYSTEM PUBLIC ${Franka_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS} ) target_include_directories(custom_franka_state_controller PUBLIC include ) ## franka_control_node add_executable(custom_franka_control_node src/franka_control_node.cpp src/motion_controller_interface.cpp ) add_dependencies(custom_franka_control_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS} ) target_link_libraries(custom_franka_control_node ${Franka_LIBRARIES} ${franka_control_LIBRARIES} # franka_control_services ${catkin_LIBRARIES} ) target_include_directories(custom_franka_control_node SYSTEM PUBLIC ${Franka_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS} include ) ## Installation install(TARGETS custom_franka_state_controller custom_franka_control_node ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} ) install(DIRECTORY config DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} ) install(FILES state_controller_plugin.xml DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} ) include(${CMAKE_CURRENT_LIST_DIR}/../cmake/ClangTools.cmake OPTIONAL RESULT_VARIABLE CLANG_TOOLS ) if(CLANG_TOOLS) file(GLOB_RECURSE SOURCES ${CMAKE_CURRENT_SOURCE_DIR}/src/*.cpp) file(GLOB_RECURSE HEADERS ${CMAKE_CURRENT_SOURCE_DIR}/include/*.h ${CMAKE_CURRENT_SOURCE_DIR}/src/*.h ) add_format_target(custom_franka_state_controller FILES ${SOURCES} ${HEADERS}) add_tidy_target(custom_franka_state_controller custom_franka_control_node FILES ${SOURCES} DEPENDS custom_franka_state_controller ) endif() catkin_python_setup() catkin_install_python(PROGRAMS scripts/enable_robot.py scripts/move_to_neutral.py scripts/simple_gripper.py tests/demo_joint_positions_keyboard.py DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) # ## Tools # include(${CMAKE_CURRENT_LIST_DIR}/../cmake/ClangTools.cmake OPTIONAL # RESULT_VARIABLE CLANG_TOOLS # ) # if(CLANG_TOOLS) # file(GLOB_RECURSE SOURCES ${CMAKE_CURRENT_SOURCE_DIR}/src/*.cpp) # file(GLOB_RECURSE HEADERS # ${CMAKE_CURRENT_SOURCE_DIR}/include/*.h # ${CMAKE_CURRENT_SOURCE_DIR}/src/*.h # ) # add_format_target(custom_franka_state_controller FILES ${SOURCES} ${HEADERS}) # add_tidy_target(custom_franka_state_controller # FILES ${SOURCES} # DEPENDS custom_franka_state_controller # ) # endif() ================================================ FILE: franka_interface/README.md ================================================ # franka_interface **See induvidual source files for more detailed documentation.** ### ArmInterface - Interface class that can monitor and control the robot - Provides all required information about robot state and end-effector state - Joint positions, velocities, and effort can be directly controlled and monitored using available methods - Smooth interpolation of joint positions possible - End-effector and Stiffness frames can be directly set (uses FrankaFramesInterface from *franka_ros_interface/franka_tools*) ### GripperInterface - Interface class to monitor and control gripper - Gripper open, close methods - Grasp, move joints methods ### RobotEnable - Interface class to reset robot when in recoverable error (use *enable_robot.py* script in *scripts/*) ### RobotParams - Collects and stores all useful information about the robot from the ROS parameter server ### Scripts - Scripts providing simple gripper actions, and robot reset are provided ================================================ FILE: franka_interface/config/basic_controllers.yaml ================================================ # load the internal joint trajectory controller, and the custom robot state controller position_joint_trajectory_controller: type: position_controllers/JointTrajectoryController joints: - panda_joint1 - panda_joint2 - panda_joint3 - panda_joint4 - panda_joint5 - panda_joint6 - panda_joint7 constraints: goal_time: 0.5 panda_joint1: goal: 0.05 panda_joint2: goal: 0.05 panda_joint3: goal: 0.05 panda_joint4: goal: 0.05 panda_joint5: goal: 0.05 panda_joint6: goal: 0.05 panda_joint7: goal: 0.05 franka_ros_interface: custom_franka_state_controller: type: franka_interface/CustomFrankaStateController publish_rate: 1000 # [Hz] joint_names: - panda_joint1 - panda_joint2 - panda_joint3 - panda_joint4 - panda_joint5 - panda_joint6 - panda_joint7 arm_id: panda ================================================ FILE: franka_interface/config/robot_config.yaml ================================================ # global robot configuration and details robot_config: joint_names: - panda_joint1 - panda_joint2 - panda_joint3 - panda_joint4 - panda_joint5 - panda_joint6 - panda_joint7 arm_id: panda # Activate rate limiter? [true|false] joint_limit_warning_threshold: 0.1 # [rad] rate_limiting: true # Cutoff frequency of the low-pass filter. Set to >= 1000 to deactivate. cutoff_frequency: 100 # Internal controller for motion generators [joint_impedance|cartesian_impedance] internal_controller: joint_impedance neutral_pose: panda_joint1: -0.017792060227770554 panda_joint2: -0.7601235411041661 panda_joint3: 0.019782607023391807 panda_joint4: -2.342050140544315 panda_joint5: 0.029840531355804868 panda_joint6: 1.5411935298621688 panda_joint7: 0.7534486589746342 joint_config: joint_acceleration_limit: panda_joint1: 15.0 # rad / sec^2 panda_joint2: 7.5 # rad / sec^2 panda_joint3: 10.0 # rad / sec^2 panda_joint4: 12.5 # rad / sec^2 panda_joint5: 15.0 # rad / sec^2 panda_joint6: 20.0 # rad / sec^2 panda_joint7: 20.0 # rad / sec^2 # panda_finger_joint1: 12.0 # rad / sec^2 # panda_finger_joint2: 12.0 # rad / sec^2 joint_velocity_limit: panda_joint1: 2.1750 # rad / sec panda_joint2: 2.1750 # rad / sec panda_joint3: 2.1750 # rad / sec panda_joint4: 2.1750 # rad / sec panda_joint5: 2.6100 # rad / sec panda_joint6: 2.6100 # rad / sec panda_joint7: 2.6100 # rad / sec # panda_finger_joint1: 2.0 # rad / sec # panda_finger_joint2: 2.0 # rad / sec joint_position_limit: lower: panda_joint1: -2.8973 # rad panda_joint2: -1.7628 # rad panda_joint3: -2.8973 # rad panda_joint4: -3.0718 # rad panda_joint5: -2.8973 # rad panda_joint6: -0.0175 # rad panda_joint7: -2.8973 # rad upper: panda_joint1: 2.8973 # rad panda_joint2: 1.7628 # rad panda_joint3: 2.8973 # rad panda_joint4: -0.0698 # rad panda_joint5: 2.8973 # rad panda_joint6: 3.7525 # rad panda_joint7: 2.8973 # rad joint_effort_limit: panda_joint1: 87 # Nm panda_joint2: 87 # Nm panda_joint3: 87 # Nm panda_joint4: 87 # Nm panda_joint5: 12 # Nm panda_joint6: 12 # Nm panda_joint7: 12 # Nm realtime_config: enforce # Configure the initial defaults for the collision behavior reflexes. collision_config: lower_torque_thresholds_acceleration: [20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0] # [Nm] upper_torque_thresholds_acceleration: [20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0] # [Nm] lower_torque_thresholds_nominal: [20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0] # [Nm] upper_torque_thresholds_nominal: [20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0] # [Nm] lower_force_thresholds_acceleration: [20.0, 20.0, 20.0, 25.0, 25.0, 25.0] # [N, N, N, Nm, Nm, Nm] upper_force_thresholds_acceleration: [20.0, 20.0, 20.0, 25.0, 25.0, 25.0] # [N, N, N, Nm, Nm, Nm] lower_force_thresholds_nominal: [20.0, 20.0, 20.0, 25.0, 25.0, 25.0] # [N, N, N, Nm, Nm, Nm] upper_force_thresholds_nominal: [20.0, 20.0, 20.0, 25.0, 25.0, 25.0] # [N, N, N, Nm, Nm, Nm] gripper_config: joint_names: - panda_finger_joint1 - panda_finger_joint2 default_speed: 0.1 # [m/s] default_grasp_epsilon: inner: 0.005 # [m] outer: 0.005 # [m] controllers_config: position_controller: "franka_ros_interface/position_joint_position_controller" torque_controller: "franka_ros_interface/effort_joint_torque_controller" impedance_controller: "franka_ros_interface/effort_joint_impedance_controller" velocity_controller: "franka_ros_interface/velocity_joint_velocity_controller" trajectory_controller: "position_joint_trajectory_controller" default_controller: "position_joint_trajectory_controller" # for safety, always set a position controller as default command_timeout: 0.2 # timeout to wait for consecutive torque control commands (or velocity) when using torque (or velocity) control. If timeout is violated, the controller interface will automatically switch to default controller for safety ================================================ FILE: franka_interface/include/franka_interface/motion_controller_interface.h ================================================ /*************************************************************************** * Adapted from sawyer_simulator package * * @package: franka_interface * @metapackage: franka_ros_interface * @author: Saif Sidhik * **************************************************************************/ /*************************************************************************** * Copyright (c) 2019-2021, Saif Sidhik. * Copyright (c) 2013-2018, Rethink Robotics Inc. * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. **************************************************************************/ #ifndef _FRANKA_INTERFACE____MOTION_CONTROLLER_INTERFACE_H_ #define _FRANKA_INTERFACE____MOTION_CONTROLLER_INTERFACE_H_ #include #include #include #include #include #include namespace franka_interface { class MotionControllerInterface { public: /** * Initializes the controller manager. * * @param[in] root_node_handle Node handle in the controller_manager namespace. * @param[in] controller_manager the controller manager instance. */ void init(ros::NodeHandle& nh, boost::shared_ptr controller_manager); private: // mutex for re-entrant calls to modeCommandCallback std::mutex mtx_; int current_mode_; realtime_tools::RealtimeBox< std::shared_ptr > box_timeout_length_; realtime_tools::RealtimeBox< std::shared_ptr > box_cmd_timeout_; ros::Timer cmd_timeout_timer_; ros::Subscriber joint_command_timeout_sub_; ros::Subscriber joint_command_sub_; boost::shared_ptr controller_manager_; std::string position_controller_name_; std::string velocity_controller_name_; std::string torque_controller_name_; std::string impedance_controller_name_; std::string trajectory_controller_name_; std::string default_controller_name_; std::string current_controller_name_; std::vector all_controllers_; std::map controller_name_to_mode_map_; protected: /** * Callback function to set time out to switch back to position mode. When using torque * mode controllers, consecutive torque commands should be sent in smaller intervals than * this timeout value * * @param[in] msg Float64 instance containing the desired timeout. */ void jointCommandTimeoutCallback(const std_msgs::Float64 msg); /** * Callback function to choose the appropriate controller. This function checks the type * of controller requested by the client through the type field in the message, and * starts the controller. * * @param[in] msg JointCommandConstPtr instance containing the joint commands. */ void jointCommandCallback(const franka_core_msgs::JointCommandConstPtr& msg); /** * Switch controller depending on the mode specified. * * @param[in] control_mode integer value representing the type of controller to use */ bool switchControllers(int control_mode); /** * Switch controller to the initially defined default controller. */ bool switchToDefaultController(); /** * Check if the command timeout has been violated. * * @param[in] control_mode integer value representing the type of controller to use */ void commandTimeoutCheck(const ros::TimerEvent& e); }; } #endif // #ifndef _FRANKA_INTERFACE____MOTION_CONTROLLER_INTERFACE_H_ ================================================ FILE: franka_interface/include/franka_interface/robot_state_controller.h ================================================ /*************************************************************************** * Adapted from robot_state_controller.cpp (franka_ros package) * * @package: franka_interface * @metapackage: franka_ros_interface * @author: Saif Sidhik * **************************************************************************/ /*************************************************************************** * Copyright (c) 2019-2021, Saif Sidhik * Copyright (c) 2017 Franka Emika GmbH * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. **************************************************************************/ #pragma once #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include namespace franka_interface { /** * Custom controller to publish the robot state as ROS topics. */ class CustomFrankaStateController : public controller_interface::MultiInterfaceController { public: /** * Creates an instance of a CustomFrankaStateController. */ CustomFrankaStateController() = default; /** * Initializes the controller with interfaces and publishers. * * @param[in] robot_hardware RobotHW instance to get a franka_hw::FrankaStateInterface from. * @param[in] root_node_handle Node handle in the controller_manager namespace. * @param[in] controller_node_handle Node handle in the controller namespace. */ bool init(hardware_interface::RobotHW* robot_hardware, ros::NodeHandle& root_node_handle, ros::NodeHandle& controller_node_handle) override; /** * Reads the current robot state from the franka_hw::FrankaStateInterface and publishes it. * * @param[in] time Current ROS time. * @param[in] period Time since the last update. */ void update(const ros::Time& time, const ros::Duration& period) override; private: void publishFrankaState(const ros::Time& time); void publishJointStates(const ros::Time& time); void publishTransforms(const ros::Time& time); void publishEndPointState(const ros::Time& time); std::string arm_id_; franka_hw::FrankaStateInterface* franka_state_interface_{}; std::unique_ptr franka_state_handle_{}; std::unique_ptr model_handle_; realtime_tools::RealtimePublisher publisher_transforms_; realtime_tools::RealtimePublisher publisher_franka_state_; realtime_tools::RealtimePublisher publisher_joint_states_; realtime_tools::RealtimePublisher publisher_joint_states_desired_; realtime_tools::RealtimePublisher publisher_tip_state_; franka_hw::TriggerRate trigger_publish_; franka::RobotState robot_state_; uint64_t sequence_number_ = 0; std::vector joint_names_; }; } // namespace franka_interface ================================================ FILE: franka_interface/launch/interface.launch ================================================ [franka_ros_interface/custom_franka_state_controller/joint_states, franka_gripper/joint_states] [franka_ros_interface/custom_franka_state_controller/joint_states] [franka_ros_interface/custom_franka_state_controller/joint_states_desired, franka_gripper/joint_states] [franka_ros_interface/custom_franka_state_controller/joint_states_desired] ================================================ FILE: franka_interface/package.xml ================================================ > franka_interface 0.7.1 ROS/Python interface classes for control of Franka Panda robot through franka-ros and libfranka. Saif Sidhik Apache 2.0 Saif Sidhik catkin message_generation eigen franka_control controller_interface dynamic_reconfigure franka_hw controller_manager franka_core_msgs franka_msgs geometry_msgs hardware_interface libfranka pluginlib realtime_tools roscpp franka_control franka_description panda_moveit_config franka_moveit rospy ================================================ FILE: franka_interface/scripts/enable_robot.py ================================================ #!/usr/bin/env python # /*************************************************************************** # # @package: franka_interface # @metapackage: franka_ros_interface # @author: Saif Sidhik # # **************************************************************************/ # /*************************************************************************** # Copyright (c) 2019-2021, Saif Sidhik # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. # You may obtain a copy of the License at # http://www.apache.org/licenses/LICENSE-2.0 # Unless required by applicable law or agreed to in writing, software # distributed under the License is distributed on an "AS IS" BASIS, # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. # See the License for the specific language governing permissions and # limitations under the License. # **************************************************************************/ """ :info: Utility script for enabling, disabling and checking status of robot. Usage: enable_robot.py :Args: : -s (--state) / -e (--enable) / -d (--disable) / -r (--reset) / -S (--stop) Not implemented: Disabling robot. Todo: Change reset request to service request instead of publishing message to topic. """ import argparse import sys import rospy import franka_interface def main(): rospy.init_node('panda_robot_enable') parser = argparse.ArgumentParser() parser.add_argument('-s', '--state', const='state', dest='actions', action='append_const', help='Print current robot state') parser.add_argument('-e', '--enable', const='enable', dest='actions', action='append_const', help='Enable the robot') parser.add_argument('-d', '--disable', const='disable', dest='actions', action='append_const', help='Disable the robot') parser.add_argument('-r', '--reset', const='reset', dest='actions', action='append_const', help='Reset the robot') parser.add_argument('-S', '--stop', const='stop', dest='actions', action='append_const', help='Stop the robot') args = parser.parse_args(rospy.myargv()[1:]) if args.actions == None: parser.print_usage() parser.exit(0, "No action defined\n") rs = franka_interface.RobotEnable() try: for act in args.actions: if act == 'state': print(rs.state()) elif act == 'enable': rs.enable() elif act == 'disable': rs.disable() elif act == 'reset': rs.reset() elif act == 'stop': rs.stop() except Exception as e: rospy.logerr(e) return 0 if __name__ == '__main__': sys.exit(main()) ================================================ FILE: franka_interface/scripts/move_to_neutral.py ================================================ #!/usr/bin/env python # /*************************************************************************** # # @package: franka_interface # @metapackage: franka_ros_interface # @author: Saif Sidhik # # **************************************************************************/ # /*************************************************************************** # Copyright (c) 2019-2021, Saif Sidhik # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. # You may obtain a copy of the License at # http://www.apache.org/licenses/LICENSE-2.0 # Unless required by applicable law or agreed to in writing, software # distributed under the License is distributed on an "AS IS" BASIS, # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. # See the License for the specific language governing permissions and # limitations under the License. # **************************************************************************/ """ :info: commands robot to move to neutral pose """ import rospy from franka_interface import ArmInterface if __name__ == '__main__': rospy.init_node("move_to_neutral_node") r = ArmInterface() r.move_to_neutral() ================================================ FILE: franka_interface/scripts/simple_gripper.py ================================================ #!/usr/bin/env python # /*************************************************************************** # # @package: franka_interface # @metapackage: franka_ros_interface # @author: Saif Sidhik # # **************************************************************************/ # /*************************************************************************** # Copyright (c) 2019-2021, Saif Sidhik # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. # You may obtain a copy of the License at # http://www.apache.org/licenses/LICENSE-2.0 # Unless required by applicable law or agreed to in writing, software # distributed under the License is distributed on an "AS IS" BASIS, # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. # See the License for the specific language governing permissions and # limitations under the License. # **************************************************************************/ """ :info: NOTE: Running this script without any arguments will stop the current command being sent to the gripper. Use this to stop any dangerous action being run Utility script for simple gripper motion. Usage: python simple_gripper.py @Args: : Stop current gripper action -o (--open) : optional argument to make the gripper release and go to max width -c (--close) : optional argument to close the gripper """ import rospy import argparse import actionlib from franka_interface import GripperInterface from franka_gripper.msg import ( StopAction, StopGoal ) def _active_cb(): rospy.loginfo("GripperInterface: '{}' request active.".format(_caller)) def _feedback_cb(msg): rospy.loginfo("GripperInterface: '{}' request feedback: \n\t{}".format(_caller,msg)) def _done_cb(status, result): rospy.loginfo("GripperInterface: '{}' complete. Result: \n\t{}".format(_caller, result)) if __name__ == '__main__': rospy.init_node("gripper_stop_node") _caller = "stop_gripper" stop_action_client = actionlib.SimpleActionClient("/franka_gripper/stop", StopAction) stop_action_client.wait_for_server() stop_action_client.send_goal(StopGoal(), done_cb =_done_cb, active_cb = _active_cb, feedback_cb = _feedback_cb) result = stop_action_client.wait_for_result(rospy.Duration(15.)) parser = argparse.ArgumentParser("Stop current gripper action; Open or close gripper.") gripper_group = parser.add_mutually_exclusive_group(required=False) gripper_group.add_argument("-o", "--open", dest="open", action='store_true', default=False, help="open gripper") gripper_group.add_argument("-c", "--close", dest="close", action='store_true', default=False, help="close gripper") args = parser.parse_args(rospy.myargv()[1:]) if args.open: gi = GripperInterface() gi.open() else: gi = GripperInterface() gi.close() ================================================ FILE: franka_interface/setup.py ================================================ #!/usr/bin/env python from distutils.core import setup from catkin_pkg.python_setup import generate_distutils_setup d = generate_distutils_setup() d['packages'] = ['franka_interface','franka_dataflow'] d['package_dir'] = {'': 'src'} setup(**d) ================================================ FILE: franka_interface/src/franka_control_node.cpp ================================================ // /*************************************************************************** // * Adapted from franka_control_node.cpp (frank_ros package) // * // * @package: franka_interface // * @metapackage: franka_ros_interface // * @author: Saif Sidhik // * // **************************************************************************/ // /*************************************************************************** // * Copyright (c) 2019-2021, Saif Sidhik. // * Copyright (c) 2017 Franka Emika GmbH // * // * Licensed under the Apache License, Version 2.0 (the "License"); // * you may not use this file except in compliance with the License. // * You may obtain a copy of the License at // * // * http://www.apache.org/licenses/LICENSE-2.0 // * // * Unless required by applicable law or agreed to in writing, software // * distributed under the License is distributed on an "AS IS" BASIS, // * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // * See the License for the specific language governing permissions and // * limitations under the License. // **************************************************************************/ #include #include #include #include #include #include #include #include #include #include #include using franka_hw::ServiceContainer; int main(int argc, char** argv) { ros::init(argc, argv, "franka_control_node"); ros::NodeHandle public_node_handle; ros::NodeHandle config_node_handle("/robot_config"); ros::NodeHandle base_node_handle("~"); franka_hw::FrankaHW franka_control; if (!franka_control.init(public_node_handle, config_node_handle)) { ROS_ERROR("franka_control_node: Failed to initialize FrankaHW class. Shutting down!"); return 1; } franka::Robot& robot = franka_control.robot(); std::atomic_bool has_error(false); ServiceContainer services; services .advertiseService(base_node_handle, "/franka_ros_interface/franka_control/set_joint_impedance", [&robot](auto&& req, auto&& res) { return franka_hw::setJointImpedance( robot, req, res); }) .advertiseService( base_node_handle, "/franka_ros_interface/franka_control/set_cartesian_impedance", [&robot](auto&& req, auto&& res) { return franka_hw::setCartesianImpedance(robot, req, res); }) .advertiseService( base_node_handle, "/franka_ros_interface/franka_control/set_EE_frame", [&robot](auto&& req, auto&& res) { return franka_hw::setEEFrame(robot, req, res); }) .advertiseService( base_node_handle, "/franka_ros_interface/franka_control/set_K_frame", [&robot](auto&& req, auto&& res) { return franka_hw::setKFrame(robot, req, res); }) .advertiseService( base_node_handle, "/franka_ros_interface/franka_control/set_force_torque_collision_behavior", [&robot](auto&& req, auto&& res) { return franka_hw::setForceTorqueCollisionBehavior(robot, req, res); }) .advertiseService( base_node_handle, "/franka_ros_interface/franka_control/set_full_collision_behavior", [&robot](auto&& req, auto&& res) { return franka_hw::setFullCollisionBehavior(robot, req, res); }) .advertiseService( base_node_handle, "/franka_ros_interface/franka_control/set_load", [&robot](auto&& req, auto&& res) { return franka_hw::setLoad(robot, req, res); }); actionlib::SimpleActionServer recovery_action_server( base_node_handle, "/franka_ros_interface/franka_control/error_recovery", [&](const franka_msgs::ErrorRecoveryGoalConstPtr&) { try { robot.automaticErrorRecovery(); has_error = false; recovery_action_server.setSucceeded(); ROS_INFO("Recovered from error"); } catch (const franka::Exception& ex) { recovery_action_server.setAborted(franka_msgs::ErrorRecoveryResult(), ex.what()); } }, false); franka_control.update(robot.readOnce()); boost::shared_ptr control_manager; control_manager.reset(new controller_manager::ControllerManager(&franka_control, public_node_handle)); franka_interface::MotionControllerInterface motion_controller_interface_; motion_controller_interface_.init(public_node_handle, control_manager); recovery_action_server.start(); // // Start background threads for message handling ros::AsyncSpinner spinner(4); spinner.start(); while (ros::ok()) { ros::Time last_time = ros::Time::now(); // Wait until controller has been activated or error has been recovered while (!franka_control.controllerActive() || has_error) { franka_control.update(robot.readOnce()); ros::Time now = ros::Time::now(); control_manager->update(now, now - last_time); franka_control.checkJointLimits(); last_time = now; if (!ros::ok()) { return 0; } } try { // Run control loop. Will exit if the controller is switched. franka_control.control([&](const ros::Time& now, const ros::Duration& period) { if (period.toSec() == 0.0) { // Reset controllers before starting a motion control_manager->update(now, period, true); franka_control.checkJointLimits(); franka_control.reset(); } else { control_manager->update(now, period); franka_control.checkJointLimits(); franka_control.enforceLimits(period); } return ros::ok(); }); } catch (const franka::ControlException& e) { ROS_ERROR("%s", e.what()); has_error = true; } } return 0; } ================================================ FILE: franka_interface/src/franka_dataflow/__init__.py ================================================ # /*************************************************************************** # Copyright (c) 2019-2021, Saif Sidhik # Copyright (c) 2013-2018, Rethink Robotics Inc. # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. # You may obtain a copy of the License at # http://www.apache.org/licenses/LICENSE-2.0 # Unless required by applicable law or agreed to in writing, software # distributed under the License is distributed on an "AS IS" BASIS, # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. # See the License for the specific language governing permissions and # limitations under the License. # **************************************************************************/ from .wait_for import wait_for ================================================ FILE: franka_interface/src/franka_dataflow/getch.py ================================================ # Copyright (c) 2013-2018, Rethink Robotics Inc. # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. # You may obtain a copy of the License at # # http://www.apache.org/licenses/LICENSE-2.0 # # Unless required by applicable law or agreed to in writing, software # distributed under the License is distributed on an "AS IS" BASIS, # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. # See the License for the specific language governing permissions and # limitations under the License. import sys import termios import tty from select import select def getch(timeout=0.01): """ Retrieves a character from stdin. Returns None if no character is available within the timeout. Blocks if timeout < 0. """ # If this is being piped to, ignore non-blocking functionality if not sys.stdin.isatty(): return sys.stdin.read(1) fileno = sys.stdin.fileno() old_settings = termios.tcgetattr(fileno) ch = None try: tty.setraw(fileno) rlist = [fileno] if timeout >= 0: [rlist, _, _] = select(rlist, [], [], timeout) if fileno in rlist: ch = sys.stdin.read(1) except Exception as ex: print("getch", ex) raise OSError finally: termios.tcsetattr(fileno, termios.TCSADRAIN, old_settings) return ch ================================================ FILE: franka_interface/src/franka_dataflow/wait_for.py ================================================ # /*************************************************************************** # Modified from: Rethink Robotics Intera SDK # # @package: franka_interface # @metapackage: franka_ros_interface # @author: Saif Sidhik # # **************************************************************************/ # /*************************************************************************** # Copyright (c) 2019-2021, Saif Sidhik # Copyright (c) 2013-2018, Rethink Robotics Inc. # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. # You may obtain a copy of the License at # http://www.apache.org/licenses/LICENSE-2.0 # Unless required by applicable law or agreed to in writing, software # distributed under the License is distributed on an "AS IS" BASIS, # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. # See the License for the specific language governing permissions and # limitations under the License. # **************************************************************************/ import errno import rospy def wait_for(test, timeout=1.0, raise_on_error=True, rate=100, timeout_msg="timeout expired", body=None): """ Waits until some condition evaluates to true. :param test: zero param function to be evaluated :param timeout: max amount of time to wait. negative/inf for indefinitely :param raise_on_error: raise or just return False :param rate: the rate at which to check :param timout_msg: message to supply to the timeout exception :param body: optional function to execute while waiting """ end_time = rospy.get_time() + timeout rate = rospy.Rate(rate) notimeout = (timeout < 0.0) or timeout == float("inf") while not test(): if rospy.is_shutdown(): if raise_on_error: raise OSError(errno.ESHUTDOWN, "ROS Shutdown") return False elif (not notimeout) and (rospy.get_time() >= end_time): if raise_on_error: raise OSError(errno.ETIMEDOUT, timeout_msg) rospy.loginfo(timeout_msg) return False if callable(body): body() rate.sleep() return True ================================================ FILE: franka_interface/src/franka_interface/__init__.py ================================================ # /*************************************************************************** # Copyright (c) 2019-2021, Saif Sidhik # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. # You may obtain a copy of the License at # http://www.apache.org/licenses/LICENSE-2.0 # Unless required by applicable law or agreed to in writing, software # distributed under the License is distributed on an "AS IS" BASIS, # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. # See the License for the specific language governing permissions and # limitations under the License. # **************************************************************************/ from .robot_params import RobotParams from .arm import ArmInterface from .gripper import GripperInterface from .robot_enable import RobotEnable ================================================ FILE: franka_interface/src/franka_interface/arm.py ================================================ # /*************************************************************************** # # @package: franka_interface # @metapackage: franka_ros_interface # @author: Saif Sidhik # # **************************************************************************/ # /*************************************************************************** # Copyright (c) 2019-2021, Saif Sidhik # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. # You may obtain a copy of the License at # http://www.apache.org/licenses/LICENSE-2.0 # Unless required by applicable law or agreed to in writing, software # distributed under the License is distributed on an "AS IS" BASIS, # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. # See the License for the specific language governing permissions and # limitations under the License. # **************************************************************************/ """ :info: Inteface Class for Franka robot arm. """ from builtins import dict # for python2&3 efficient compatibility from future.utils import iteritems # for python2&3 efficient compatibility import enum import rospy import warnings import quaternion import numpy as np from copy import deepcopy from rospy_message_converter import message_converter from franka_core_msgs.msg import JointCommand from franka_core_msgs.msg import RobotState, EndPointState from sensor_msgs.msg import JointState from std_msgs.msg import Float64 import franka_dataflow from .robot_params import RobotParams from franka_moveit import PandaMoveGroupInterface from franka_moveit.utils import create_pose_msg from franka_tools import FrankaFramesInterface, FrankaControllerManagerInterface, JointTrajectoryActionClient, CollisionBehaviourInterface class TipState(): def __init__(self, timestamp, pose, vel, O_effort, K_effort): self.timestamp = timestamp self._pose = pose self._velocity = vel self._effort = O_effort self._effort_in_K_frame = K_effort @property def pose(self): return self._pose @property def velocity(self): return self._velocity @property def effort(self): return self._effort @property def effort_in_K_frame(self): return self._effort_in_K_frame class ArmInterface(object): """ Interface Class for an arm of Franka Panda robot. Constructor. :type synchronous_pub: bool :param synchronous_pub: designates the JointCommand Publisher as Synchronous if True and Asynchronous if False. Synchronous Publishing means that all joint_commands publishing to the robot's joints will block until the message has been serialized into a buffer and that buffer has been written to the transport of every current Subscriber. This yields predicable and consistent timing of messages being delivered from this Publisher. However, when using this mode, it is possible for a blocking Subscriber to prevent the joint_command functions from exiting. Unless you need exact JointCommand timing, default to Asynchronous Publishing (False). """ # Containers @enum.unique class RobotMode(enum.IntEnum): """ Enum class for specifying and retrieving the current robot mode. """ # ----- access using parameters name or value # ----- eg. RobotMode(0).name & RobotMode(0).value # ----- or RobotMode['ROBOT_MODE_OTHER'].name & RobotMode['ROBOT_MODE_OTHER'].value ROBOT_MODE_OTHER = 0 ROBOT_MODE_IDLE = 1 ROBOT_MODE_MOVE = 2 ROBOT_MODE_GUIDING = 3 ROBOT_MODE_REFLEX = 4 ROBOT_MODE_USER_STOPPED = 5 ROBOT_MODE_AUTOMATIC_ERROR_RECOVERY = 6 def __init__(self, synchronous_pub=False): self._params = RobotParams() self._ns = self._params.get_base_namespace() self._joint_limits = self._params.get_joint_limits() joint_names = self._joint_limits.joint_names if not joint_names: rospy.logerr("{}: Cannot detect joint names for arm on this " "robot. Exiting Arm.init().".format(self.__class__.__name__)) return self._joint_names = joint_names self.name = self._params.get_robot_name() self._joint_angle = dict() self._joint_velocity = dict() self._joint_effort = dict() self._cartesian_pose = dict() self._cartesian_velocity = dict() self._cartesian_effort = dict() self._stiffness_frame_effort = dict() self._errors = dict() self._collision_state = False self._tip_states = None self._jacobian = None self._cartesian_contact = None self._robot_mode = False self._command_msg = JointCommand() # neutral pose joint positions self._neutral_pose_joints = self._params.get_neutral_pose() self._frames_interface = FrankaFramesInterface() try: self._collision_behaviour_interface = CollisionBehaviourInterface() except rospy.ROSException: rospy.loginfo("{}: Collision Service Not found. It will not be possible to change collision behaviour of robot!".format( self.__class__.__name__)) self._collision_behaviour_interface = None self._ctrl_manager = FrankaControllerManagerInterface( ns=self._ns, sim=self._params._in_sim) self._speed_ratio = 0.15 self._F_T_NE = np.eye(1).flatten().tolist() self._NE_T_EE = np.eye(1).flatten().tolist() queue_size = None if synchronous_pub else 1 with warnings.catch_warnings(): warnings.simplefilter("ignore") self._joint_command_publisher = rospy.Publisher( self._ns + '/motion_controller/arm/joint_commands', JointCommand, tcp_nodelay=True, queue_size=queue_size) self._pub_joint_cmd_timeout = rospy.Publisher( self._ns + '/motion_controller/arm/joint_command_timeout', Float64, latch=True, queue_size=10) self._robot_state_subscriber = rospy.Subscriber( self._ns + '/custom_franka_state_controller/robot_state', RobotState, self._on_robot_state, queue_size=1, tcp_nodelay=True) joint_state_topic = self._ns + '/custom_franka_state_controller/joint_states' self._joint_state_sub = rospy.Subscriber( joint_state_topic, JointState, self._on_joint_states, queue_size=1, tcp_nodelay=True) self._cartesian_state_sub = rospy.Subscriber( self._ns + '/custom_franka_state_controller/tip_state', EndPointState, self._on_endpoint_state, queue_size=1, tcp_nodelay=True) rospy.on_shutdown(self._clean_shutdown) err_msg = ("%s arm init failed to get current joint_states " "from %s") % (self.name.capitalize(), joint_state_topic) franka_dataflow.wait_for(lambda: len(list(self._joint_angle.keys())) > 0, timeout_msg=err_msg, timeout=5.0) err_msg = ("%s arm, init failed to get current tip_state " "from %s") % (self.name.capitalize(), self._ns + 'tip_state') franka_dataflow.wait_for(lambda: len(list(self._cartesian_pose.keys())) > 0, timeout_msg=err_msg, timeout=5.0) err_msg = ("%s arm, init failed to get current robot_state " "from %s") % (self.name.capitalize(), self._ns + 'robot_state') franka_dataflow.wait_for(lambda: self._jacobian is not None, timeout_msg=err_msg, timeout=5.0) # start moveit server with panda_link8 (flange) as the end-effector, unless it is in simulation. However, using move_to_cartesian_pose() method compensates for this and moves the robot's defined end-effector frame (EE frame; see set_EE_frame() and set_EE_at_frame()) to the target pose. try: self._movegroup_interface = PandaMoveGroupInterface( use_panda_hand_link=True if self._params._in_sim else False) except: rospy.loginfo("{}: MoveGroup was not found! This is okay if moveit service is not required!".format( self.__class__.__name__)) self._movegroup_interface = None self.set_joint_position_speed(self._speed_ratio) def _clean_shutdown(self): self._joint_state_sub.unregister() self._cartesian_state_sub.unregister() self._pub_joint_cmd_timeout.unregister() self._robot_state_subscriber.unregister() self._joint_command_publisher.unregister() def get_movegroup_interface(self): """ :return: the movegroup interface instance associated with the robot. :rtype: franka_moveit.PandaMoveGroupInterface """ return self._movegroup_interface def get_robot_params(self): """ :return: Useful parameters from the ROS parameter server. :rtype: franka_interface.RobotParams """ return self._params def get_joint_limits(self): """ Return the joint limits (defined in the parameter server) :rtype: franka_core_msgs.msg.JointLimits :return: JointLimits """ return self._joint_limits def joint_names(self): """ Return the names of the joints for the specified limb. :rtype: [str] :return: ordered list of joint names from proximal to distal (i.e. shoulder to wrist). """ return self._joint_names def _on_joint_states(self, msg): for idx, name in enumerate(msg.name): if name in self._joint_names: self._joint_angle[name] = msg.position[idx] self._joint_velocity[name] = msg.velocity[idx] self._joint_effort[name] = msg.effort[idx] def _on_robot_state(self, msg): self._robot_mode = self.RobotMode(msg.robot_mode) self._robot_mode_ok = (self._robot_mode.value != self.RobotMode.ROBOT_MODE_REFLEX) and ( self._robot_mode.value != self.RobotMode.ROBOT_MODE_USER_STOPPED) self._jacobian = np.asarray(msg.O_Jac_EE).reshape(6, 7, order='F') self._cartesian_velocity = { 'linear': np.asarray([msg.O_dP_EE[0], msg.O_dP_EE[1], msg.O_dP_EE[2]]), 'angular': np.asarray([msg.O_dP_EE[3], msg.O_dP_EE[4], msg.O_dP_EE[5]])} self._cartesian_contact = msg.cartesian_contact self._cartesian_collision = msg.cartesian_collision self._joint_contact = msg.joint_contact self._joint_collision = msg.joint_collision if not self._params._in_sim: self._F_T_NE = msg.F_T_NE # should be constant normally self._NE_T_EE = msg.NE_T_EE self._F_T_EE = msg.F_T_EE if self._frames_interface: self._frames_interface._update_frame_data( self._NE_T_EE, msg.EE_T_K) self._joint_inertia = np.asarray( msg.mass_matrix).reshape(7, 7, order='F') self.q_d = msg.q_d self.dq_d = msg.dq_d self._gravity = np.asarray(msg.gravity) self._coriolis = np.asarray(msg.coriolis) self._errors = message_converter.convert_ros_message_to_dictionary( msg.current_errors) def coriolis_comp(self): """ Return coriolis compensation torques. Useful for compensating coriolis when performing direct torque control of the robot. :rtype: np.ndarray :return: 7D joint torques compensating for coriolis. """ return self._coriolis def gravity_comp(self): """ Return gravity compensation torques. :rtype: np.ndarray :return: 7D joint torques compensating for gravity. """ return self._gravity def get_robot_status(self): """ Return dict with all robot status information. :rtype: dict :return: ['robot_mode' (RobotMode object), 'robot_status' (bool), 'errors' (dict() of errors and their truth value), 'error_in_curr_status' (bool)] """ return {'robot_mode': self._robot_mode, 'robot_status': self._robot_mode_ok, 'errors': self._errors, 'error_in_current_state': self.error_in_current_state()} def in_safe_state(self): """ Return True if the specified limb is in safe state (no collision, reflex, errors etc.). :rtype: bool :return: True if the arm is in safe state, False otherwise. """ return self._robot_mode_ok and not self.error_in_current_state() def error_in_current_state(self): """ Return True if the specified limb has experienced an error. :rtype: bool :return: True if the arm has error, False otherwise. """ return not all([e == False for e in list(self._errors.values())]) def what_errors(self): """ Return list of error messages if there is error in robot state :rtype: [str] :return: list of names of current errors in robot state """ return [e for e in self._errors if self._errors[e] == True] if self.error_in_current_state() else None def _on_endpoint_state(self, msg): cart_pose_trans_mat = np.asarray(msg.O_T_EE).reshape(4, 4, order='F') self._cartesian_pose = { 'position': cart_pose_trans_mat[:3, 3], 'orientation': quaternion.from_rotation_matrix(cart_pose_trans_mat[:3, :3]), 'ori_mat': cart_pose_trans_mat[:3,:3]} self._cartesian_effort = { 'force': np.asarray([msg.O_F_ext_hat_K.wrench.force.x, msg.O_F_ext_hat_K.wrench.force.y, msg.O_F_ext_hat_K.wrench.force.z]), 'torque': np.asarray([msg.O_F_ext_hat_K.wrench.torque.x, msg.O_F_ext_hat_K.wrench.torque.y, msg.O_F_ext_hat_K.wrench.torque.z]) } self._stiffness_frame_effort = { 'force': np.asarray([msg.K_F_ext_hat_K.wrench.force.x, msg.K_F_ext_hat_K.wrench.force.y, msg.K_F_ext_hat_K.wrench.force.z]), 'torque': np.asarray([msg.K_F_ext_hat_K.wrench.torque.x, msg.K_F_ext_hat_K.wrench.torque.y, msg.K_F_ext_hat_K.wrench.torque.z]) } self._tip_states = TipState(msg.header.stamp, deepcopy(self._cartesian_pose), deepcopy( self._cartesian_velocity), deepcopy(self._cartesian_effort), deepcopy(self._stiffness_frame_effort)) def joint_angle(self, joint): """ Return the requested joint angle. :type joint: str :param joint: name of a joint :rtype: float :return: angle in radians of individual joint """ return self._joint_angle[joint] def joint_angles(self): """ Return all joint angles. :rtype: dict({str:float}) :return: unordered dict of joint name Keys to angle (rad) Values """ return deepcopy(self._joint_angle) def joint_ordered_angles(self): """ Return all joint angles. :rtype: [float] :return: joint angles (rad) orded by joint_names from proximal to distal (i.e. shoulder to wrist). """ return [self._joint_angle[name] for name in self._joint_names] def joint_velocity(self, joint): """ Return the requested joint velocity. :type joint: str :param joint: name of a joint :rtype: float :return: velocity in radians/s of individual joint """ return self._joint_velocity[joint] def joint_velocities(self): """ Return all joint velocities. :rtype: dict({str:float}) :return: unordered dict of joint name Keys to velocity (rad/s) Values """ return deepcopy(self._joint_velocity) def joint_effort(self, joint): """ Return the requested joint effort. :type joint: str :param joint: name of a joint :rtype: float :return: effort in Nm of individual joint """ return self._joint_effort[joint] def joint_efforts(self): """ Return all joint efforts. :rtype: dict({str:float}) :return: unordered dict of joint name Keys to effort (Nm) Values """ return deepcopy(self._joint_effort) def endpoint_pose(self): """ Return Cartesian endpoint pose {position, orientation}. :rtype: dict({str:np.ndarray (shape:(3,)), str:quaternion.quaternion}) :return: position and orientation as named tuples in a dict - 'position': np.array of x, y, z - 'orientation': quaternion x,y,z,w in quaternion format """ return deepcopy(self._cartesian_pose) def endpoint_velocity(self): """ Return Cartesian endpoint twist {linear, angular}. :rtype: dict({str:np.ndarray (shape:(3,)),str:np.ndarray (shape:(3,))}) :return: linear and angular velocities as named tuples in a dict - 'linear': np.array of x, y, z - 'angular': np.array of x, y, z (angular velocity along the axes) """ return deepcopy(self._cartesian_velocity) def endpoint_effort(self, in_base_frame=True): """ Return Cartesian endpoint wrench {force, torque}. :param in_base_frame: if True, returns end-effector effort with respect to base frame, else in stiffness frame [default: True] :type in_base_frame: bool :rtype: dict({str:np.ndarray (shape:(3,)),str:np.ndarray (shape:(3,))}) :return: force and torque at endpoint as named tuples in a dict in the base frame of the robot or in the stiffness frame (wrist) - 'force': Cartesian force on x,y,z axes in np.ndarray format - 'torque': Torque around x,y,z axes in np.ndarray format """ return deepcopy(self._cartesian_effort) if in_base_frame else deepcopy(self._stiffness_frame_effort) def exit_control_mode(self, timeout=5, velocity_tolerance=1e-2): """ Clean exit from advanced control modes (joint torque or velocity). Resets control to joint position mode with current positions until further advanced control commands are sent to the robot. .. note:: In normal cases, this method is not required as the interface automatically switches to position control mode if advanced control commands (velocity/torque) are not sent at regular intervals. Therefore it is enough to stop sending the commands to disable advanced control modes. .. note:: In sim, this method does nothing. :type timeout: float :param timeout: seconds to wait for robot to stop moving before giving up [default: 5] :type velocity_tolerance: float :param velocity_tolerance: tolerance """ if self._params._in_sim: return self.set_command_timeout(0.05) rospy.sleep(0.5) def check_stop(): return np.allclose(np.asarray(list(self._joint_velocity.values())), 0., atol=velocity_tolerance) rospy.loginfo("{}: Waiting for robot to stop moving to exit control mode...".format( self.__class__.__name__)) franka_dataflow.wait_for( test=lambda: check_stop(), timeout=timeout, timeout_msg="{}: FAILED to exit control mode! The robot may be still moving. Controllers might not switch correctly".format( self.__class__.__name__), rate=20, raise_on_error=False ) rospy.loginfo("{}: Done. Setting position control target to current position.".format( self.__class__.__name__)) self.set_joint_positions(self.joint_angles()) def tip_states(self): """ Return Cartesian endpoint state for a given tip name :rtype: TipState object :return: pose, velocity, effort, effort_in_K_frame """ return deepcopy(self._tip_states) def joint_inertia_matrix(self): """ Returns the current joint inertia matrix given by libfranka. :return: joint inertia matrix (7,7) :rtype: np.ndarray [7x7] """ return deepcopy(self._joint_inertia) def zero_jacobian(self): """ Returns the current jacobian matrix given by libfranka. :return: end-effector jacobian (6,7) :rtype: np.ndarray [6x7] """ return deepcopy(self._jacobian) def set_command_timeout(self, timeout): """ Set the timeout in seconds for the joint controller. Advanced control commands (vel, torque) should be sent within this time, or else the controller will switch back to default control mode. :type timeout: float :param timeout: timeout in seconds """ self._pub_joint_cmd_timeout.publish(Float64(timeout)) def set_joint_position_speed(self, speed=0.3): """ Set ratio of max joint speed to use during joint position moves (only for move_to_joint_positions). Set the proportion of maximum controllable velocity to use during joint position control execution. The default ratio is `0.3`, and can be set anywhere from [0.0-1.0] (clipped). Once set, a speed ratio will persist until a new execution speed is set. :type speed: float :param speed: ratio of maximum joint speed for execution default= 0.3; range= [0.0-1.0] """ if speed > 0.3: rospy.logwarn("{}: Setting speed above 0.3 could be risky!! Be extremely careful.".format( self.__class__.__name__)) if self._movegroup_interface: self._movegroup_interface.set_velocity_scale(speed * 2) self._speed_ratio = speed def set_joint_positions(self, positions): """ Commands the joints of this limb to the specified positions. :type positions: dict({str:float} :param positions: dict of {'joint_name':joint_position,} """ self._command_msg.names = self._joint_names self._command_msg.position = [positions[j] for j in self._joint_names] self._command_msg.mode = JointCommand.POSITION_MODE self._command_msg.header.stamp = rospy.Time.now() self._joint_command_publisher.publish(self._command_msg) def set_joint_velocities(self, velocities): """ Commands the joints of this limb to the specified velocities. :type velocities: dict({str:float}) :param velocities: dict of {'joint_name':joint_velocity,} """ self._command_msg.names = self._joint_names self._command_msg.velocity = [velocities[j] for j in self._joint_names] self._command_msg.mode = JointCommand.VELOCITY_MODE self._command_msg.header.stamp = rospy.Time.now() self._joint_command_publisher.publish(self._command_msg) def set_joint_torques(self, torques): """ Commands the joints of this limb with the specified torques. :type torques: dict({str:float}) :param torques: dict of {'joint_name':joint_torque,} """ self._command_msg.names = self._joint_names self._command_msg.effort = [torques[j] for j in self._joint_names] self._command_msg.mode = JointCommand.TORQUE_MODE self._command_msg.header.stamp = rospy.Time.now() self._joint_command_publisher.publish(self._command_msg) def set_joint_positions_velocities(self, positions, velocities): """ Commands the joints of this limb using specified positions and velocities using impedance control. Command at time t is computed as: :math:`u_t= coriolis\_factor * coriolis\_t + K\_p * (positions - curr\_positions) + K\_d * (velocities - curr\_velocities)` :type positions: [float] :param positions: desired joint positions as an ordered list corresponding to joints given by self.joint_names() :type velocities: [float] :param velocities: desired joint velocities as an ordered list corresponding to joints given by self.joint_names() """ self._command_msg.names = self._joint_names self._command_msg.position = positions self._command_msg.velocity = velocities self._command_msg.mode = JointCommand.IMPEDANCE_MODE self._command_msg.header.stamp = rospy.Time.now() self._joint_command_publisher.publish(self._command_msg) def has_collided(self): """ Returns true if either joint collision or cartesian collision is detected. Collision thresholds can be set using instance of :py:class:`franka_tools.CollisionBehaviourInterface`. """ return any(self._joint_collision) or any(self._cartesian_collision) def move_to_neutral(self, timeout=15.0, speed=0.15): """ Command the Limb joints to a predefined set of "neutral" joint angles. From rosparam /franka_control/neutral_pose. :type timeout: float :param timeout: seconds to wait for move to finish [15] :type speed: float :param speed: ratio of maximum joint speed for execution default= 0.15; range= [0.0-1.0] """ self.set_joint_position_speed(speed) self.move_to_joint_positions(self._neutral_pose_joints, timeout) def move_to_joint_positions(self, positions, timeout=10.0, threshold=0.00085, test=None, use_moveit=True): """ (Blocking) Commands the limb to the provided positions. Waits until the reported joint state matches that specified. This function uses a low-pass filter using JointTrajectoryService to smooth the movement or optionally uses MoveIt! to plan and execute a trajectory. :type positions: dict({str:float}) :param positions: joint_name:angle command :type timeout: float :param timeout: seconds to wait for move to finish [15] :type threshold: float :param threshold: position threshold in radians across each joint when move is considered successful [0.00085] :param test: optional function returning True if motion must be aborted :type use_moveit: bool :param use_moveit: if set to True, and movegroup interface is available, move to the joint positions using moveit planner. """ curr_controller = self._ctrl_manager.set_motion_controller( self._ctrl_manager.joint_trajectory_controller) if use_moveit and self._movegroup_interface: self._movegroup_interface.go_to_joint_positions( [positions[n] for n in self._joint_names], tolerance=threshold) else: if use_moveit: rospy.logwarn("{}: MoveGroupInterface was not found! Using JointTrajectoryActionClient instead.".format( self.__class__.__name__)) min_traj_dur = 0.5 traj_client = JointTrajectoryActionClient( joint_names=self.joint_names()) traj_client.clear() dur = [] for j in range(len(self._joint_names)): dur.append(max(abs(positions[self._joint_names[j]] - self._joint_angle[self._joint_names[j]] ) / self._joint_limits.velocity[j], min_traj_dur)) traj_client.add_point( positions=[self._joint_angle[n] for n in self._joint_names], time=0.0001) traj_client.add_point(positions=[ positions[n] for n in self._joint_names], time=max(dur)/self._speed_ratio) def genf(joint, angle): def joint_diff(): return abs(angle - self._joint_angle[joint]) return joint_diff diffs = [genf(j, a) for j, a in list(iteritems(positions)) if j in self._joint_angle] fail_msg = "{}: {} limb failed to reach commanded joint positions.".format( self.__class__.__name__, self.name.capitalize()) def test_collision(): if self.has_collided(): rospy.logerr(' '.join(["Collision detected.", fail_msg])) return True return False traj_client.start() # send the trajectory action request # traj_client.wait(timeout = timeout) franka_dataflow.wait_for( test=lambda: test_collision() or traj_client.result() is not None or (callable(test) and test() == True) or (all(diff() < threshold for diff in diffs)), timeout=timeout, timeout_msg=fail_msg, rate=100, raise_on_error=False ) res = traj_client.result() if res is not None and res.error_code: rospy.loginfo("Trajectory Server Message: {}".format(res)) rospy.sleep(0.5) self._ctrl_manager.set_motion_controller(curr_controller) rospy.loginfo("{}: Trajectory controlling complete".format( self.__class__.__name__)) def get_flange_pose(self, pos=None, ori=None): """ Get the pose of flange (panda_link8) given the pose of the end-effector frame. .. note:: In sim, this method does nothing. :param pos: position of the end-effector frame in the robot's base frame, defaults to current end-effector position :type pos: np.ndarray, optional :param ori: orientation of the end-effector frame, defaults to current end-effector orientation :type ori: quaternion.quaternion, optional :return: corresponding flange frame pose in the robot's base frame :rtype: np.ndarray, quaternion.quaternion """ if pos is None: pos = self._cartesian_pose['position'] if ori is None: ori = self._cartesian_pose['orientation'] if self._params._in_sim: return pos, ori # get corresponding flange frame pose using transformation matrix F_T_EE = np.asarray(self._F_T_EE).reshape(4, 4, order="F") mat = quaternion.as_rotation_matrix(ori) new_ori = mat.dot(F_T_EE[:3,:3].T) new_pos = pos - new_ori.dot(F_T_EE[:3, 3]) return new_pos, quaternion.from_rotation_matrix(new_ori) def move_to_cartesian_pose(self, pos, ori=None, use_moveit=True): """ Move robot end-effector to specified cartesian pose using MoveIt! (also avoids obstacles if they are defined using :py:class:`franka_moveit.ExtendedPlanningSceneInterface`) :param pos: target end-effector position :type pos: [float] or np.ndarray :param ori: target orientation quaternion for end-effector, defaults to current ori :type ori: quaternion.quaternion or [float] (quaternion in w,x,y,z order), optional :param use_moveit: Flag for using MoveIt (redundant for now; only works if set to True), defaults to True :type use_moveit: bool, optional """ if not use_moveit or self._movegroup_interface is None: rospy.logerr("{}: MoveGroupInterface was not found! Aborting cartesian planning.".format( self.__class__.__name__)) return if ori is None: ori = self._cartesian_pose['orientation'] self.get_flange_pose(pos, ori) curr_controller = self._ctrl_manager.set_motion_controller( self._ctrl_manager.joint_trajectory_controller) ## == Plan avoids defined scene obstacles == self._movegroup_interface.go_to_cartesian_pose( create_pose_msg(*self.get_flange_pose(pos, ori))) ## ========================================= ## == does not avoid scene obstacles == # plan, _ = self._movegroup_interface.plan_cartesian_path( # [create_pose_msg(pos, ori)]) # self._movegroup_interface.execute_plan(plan) ## ==================================== rospy.sleep(0.5) self._ctrl_manager.set_motion_controller(curr_controller) rospy.loginfo("{}: Trajectory controlling complete".format( self.__class__.__name__)) def pause_controllers_and_do(self, func, *args, **kwargs): """ Temporarily stops all active controllers and calls the provided function before restarting the previously active controllers. :param func: the function to be called :type func: callable """ assert callable( func), "{}: Invalid argument provided to ArmInterface->pause_controllers_and_do. Argument 1 should be callable.".format(self.__class__.__name__) active_controllers = self._ctrl_manager.list_active_controllers( only_motion_controllers=True) rospy.loginfo("{}: Stopping motion controllers temporarily...".format( self.__class__.__name__)) for ctrlr in active_controllers: self._ctrl_manager.stop_controller(ctrlr.name) rospy.sleep(1.) retval = func(*args, **kwargs) rospy.sleep(1.) rospy.loginfo("{}: Restarting previously active motion controllers.".format( self.__class__.__name__)) for ctrlr in active_controllers: self._ctrl_manager.start_controller(ctrlr.name) rospy.sleep(1.) rospy.loginfo("{}: Controllers restarted.".format( self.__class__.__name__)) return retval def reset_EE_frame(self): """ Reset EE frame to default. (defined by FrankaFramesInterface.DEFAULT_TRANSFORMATIONS.EE_FRAME global variable defined in :py:class:`franka_tools.FrankaFramesInterface` source code). By default, this resets to align with the nominal-end effector frame (F_T_NE) in the flange frame. :rtype: [bool, str] :return: [success status of service request, error msg if any] """ if self._frames_interface: if self._frames_interface.EE_frame_is_reset(): rospy.loginfo("{}: EE Frame already reset".format( self.__class__.__name__)) return True return self.pause_controllers_and_do(self._frames_interface.reset_EE_frame) else: rospy.logwarn("{}: Frames changing not available in simulated environment".format( self.__class__.__name__)) return False def set_EE_frame(self, frame): """ Set new EE frame based on the transformation given by 'frame', which is the transformation matrix defining the new desired EE frame with respect to the nominal end-effector frame (NE_T_EE). Motion controllers are stopped and restarted for switching. :type frame: [float (16,)] / np.ndarray (4x4) :param frame: transformation matrix of new EE frame wrt nominal end-effector frame (column major) :rtype: [bool, str] :return: [success status of service request, error msg if any] """ if self._frames_interface: if self._frames_interface.frames_are_same(self._frames_interface.get_EE_frame(as_mat=True), frame): rospy.loginfo("{}: EE Frame already at the target frame.".format( self.__class__.__name__)) return True return self.pause_controllers_and_do(self._frames_interface.set_EE_frame, frame) else: rospy.logwarn("{}: Frames changing not available in simulated environment".format( self.__class__.__name__)) return False def set_EE_at_frame(self, frame_name, timeout=5.0): """ Set new EE frame to the same frame as the link frame given by 'frame_name'. Motion controllers are stopped and restarted for switching :type frame_name: str :param frame_name: desired tf frame name in the tf tree :rtype: [bool, str] :return: [success status of service request, error msg if any] """ if self._frames_interface: if not self._frames_interface.EE_frame_already_set(self._frames_interface.get_link_tf(frame_name)): return self.pause_controllers_and_do(self._frames_interface.set_EE_at_frame, frame_name=frame_name, timeout=timeout) else: rospy.logwarn("{}: Frames changing not available in simulated environment".format( self.__class__.__name__)) return False def set_collision_threshold(self, cartesian_forces=None, joint_torques=None): """ Set Force Torque thresholds for deciding robot has collided. :return: True if service call successful, False otherwise :rtype: bool :param cartesian_forces: Cartesian force threshold for collision detection [x,y,z,R,P,Y] (robot motion stops if violated) :type cartesian_forces: [float] size 6 :param joint_torques: Joint torque threshold for collision (robot motion stops if violated) :type joint_torques: [float] size 7 """ if self._collision_behaviour_interface: return self._collision_behaviour_interface.set_collision_threshold(joint_torques=joint_torques, cartesian_forces=cartesian_forces) else: rospy.logwarn("No CollisionBehaviourInterface object found!") def get_controller_manager(self): """ :return: the FrankaControllerManagerInterface instance associated with the robot. :rtype: franka_tools.FrankaControllerManagerInterface """ return self._ctrl_manager def get_frames_interface(self): """ :return: the FrankaFramesInterface instance associated with the robot. :rtype: franka_tools.FrankaFramesInterface """ return self._frames_interface if __name__ == '__main__': rospy.init_node('test_fri') r = Arm() # rate = rospy.Rate(10) # while not rospy.is_shutdown(): # rate.sleep() ================================================ FILE: franka_interface/src/franka_interface/gripper.py ================================================ # /*************************************************************************** # # @package: franka_interface # @metapackage: franka_ros_interface # @author: Saif Sidhik # # **************************************************************************/ # /*************************************************************************** # Copyright (c) 2019-2021, Saif Sidhik # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. # You may obtain a copy of the License at # http://www.apache.org/licenses/LICENSE-2.0 # Unless required by applicable law or agreed to in writing, software # distributed under the License is distributed on an "AS IS" BASIS, # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. # See the License for the specific language governing permissions and # limitations under the License. # **************************************************************************/ """ :info: Inteface Class for Franka gripper. """ from builtins import dict # for python2&3 efficient compatibility import rospy import actionlib import franka_dataflow from copy import deepcopy from sensor_msgs.msg import JointState from franka_gripper.msg import ( GraspAction, GraspGoal, HomingAction, HomingGoal, MoveAction, MoveGoal, StopAction, StopGoal, GraspEpsilon ) class GripperInterface(object): """ Interface class for the gripper on the Franka Panda robot. :param gripper_joint_names: Names of the finger joints :param ns: base namespace of interface ('frank_ros_interface'/'panda_simulator') :param calibrate: Attempts to calibrate the gripper when initializing class (defaults True) :type calibrate: bool :type gripper_joint_names: [str] :type ns: str """ def __init__(self, gripper_joint_names = ('panda_finger_joint1', 'panda_finger_joint2'), calibrate = False, **kwargs): """ Constructor. """ self.name = '/franka_gripper' ns = self.name +'/' self._joint_positions = dict() self._joint_names = gripper_joint_names self._joint_velocity = dict() self._joint_effort = dict() self._joint_states_state_sub = rospy.Subscriber(ns + 'joint_states', JointState, self._joint_states_callback, queue_size = 1, tcp_nodelay = True) self._exists = False # ----- Initial test to see if gripper is loaded try: rospy.get_param("/franka_gripper/robot_ip") except KeyError: rospy.loginfo("FrankaGripper: could not detect gripper.") return except (socket.error, socket.gaierror): print ("Failed to connect to the ROS parameter server!\n" "Please check to make sure your ROS networking is " "properly configured:\n") sys.exit() # ----- Wait for the gripper device status to be true if not franka_dataflow.wait_for(lambda: len(list(self._joint_positions.keys())) > 0, timeout=2.0, timeout_msg=("FrankaGripper: Failed to get gripper joint positions. Assuming no gripper attached to robot."), raise_on_error = False ): return self._exists = True self._gripper_speed = 0.05 self._homing_action_client = actionlib.SimpleActionClient("{}homing".format(ns), HomingAction) self._grasp_action_client = actionlib.SimpleActionClient("{}grasp".format(ns), GraspAction) self._move_action_client = actionlib.SimpleActionClient("{}move".format(ns), MoveAction) self._stop_action_client = actionlib.SimpleActionClient("{}stop".format(ns), StopAction) rospy.loginfo("GripperInterface: Waiting for gripper action servers... ") self._homing_action_client.wait_for_server() self._grasp_action_client.wait_for_server() self._move_action_client.wait_for_server() self._stop_action_client.wait_for_server() rospy.loginfo("GripperInterface: Gripper action servers found! ") self.MIN_FORCE = 0.01 self.MAX_FORCE = 50 # documentation says upto 70N is possible as continuous force (max upto 140N) self.MIN_WIDTH = 0.0001 self.MAX_WIDTH = 0.2 if calibrate: self.calibrate() @property def exists(self): """ Check if a gripper was identified as connected to the robot. :return: True if gripper was detected, False otherwise :rtype: bool """ return self._exists def set_velocity(self, value): """ Set default value for gripper joint motions. Used for move and grasp commands. :param value: speed value [m/s] :type value: float """ assert self.MIN_WIDTH <= value <= self.MAX_WIDTH, "GripperInterface: Invalid speed request for gripper joints. Should be within {} and {}.".format(self.MIN_WIDTH, self.MAX_WIDTH) self._gripper_speed = value def _joint_states_callback(self, msg): for idx, name in enumerate(msg.name): if name in self._joint_names: self._joint_positions[name] = msg.position[idx] self._joint_velocity[name] = msg.velocity[idx] self._joint_effort[name] = msg.effort[idx] def joint_names(self): """ Return the names of the joints for the specified limb. :rtype: [str] :return: ordered list of joint names. """ return self._joint_names def joint_position(self, joint): """ Return the requested joint position. :param joint: name of a joint :type joint: str :rtype: float :return: position individual joint """ return self._joint_positions[joint] def joint_positions(self): """ Return all joint positions. :rtype: dict({str:float}) :return: unordered dict of joint name Keys to pos """ return deepcopy(self._joint_positions) def joint_ordered_positions(self): """ Return all joint positions. :rtype: [double] :return: joint positions ordered by joint_names. """ return [self._joint_positions[name] for name in self._joint_names] def joint_velocity(self, joint): """ Return the requested joint velocity. :param joint: name of a joint :type joint: str :rtype: float :return: velocity in radians/s of individual joint """ return self._joint_velocity[joint] def joint_velocities(self): """ Return all joint velocities. :rtype: dict({str:float}) :return: unordered dict of joint name Keys to velocity (rad/s) Values """ return deepcopy(self._joint_velocity) def joint_ordered_velocities(self): """ Return all joint velocities. :rtype: [double] :return: joint velocities ordered by joint_names. """ return [self._joint_velocity[name] for name in self._joint_names] def joint_effort(self, joint): """ Return the requested joint effort. :param joint: name of a joint :type joint: str :rtype: float :return: effort in Nm of individual joint """ return self._joint_effort[joint] def joint_efforts(self): """ Return all joint efforts. :rtype: dict({str:float}) :return: unordered dict of joint name Keys to effort (Nm) Values """ return deepcopy(self._joint_effort) def joint_ordered_efforts(self): """ Return all joint efforts. :rtype: [double] :return: joint efforts ordered by joint_names. """ return [self._joint_effort[name] for name in self._joint_names] def _active_cb(self): rospy.logdebug("GripperInterface: '{}' request active.".format(self._caller)) def _feedback_cb(self, msg): rospy.logdebug("GripperInterface: '{}' request feedback: \n\t{}".format(self._caller,msg)) def _done_cb(self, status, result): rospy.logdebug("GripperInterface: '{}' complete. Result: \n\t{}".format(self._caller, result)) def home_joints(self, wait_for_result = False): """ Performs homing of the gripper. After changing the gripper fingers, a homing needs to be done. This is needed to estimate the maximum grasping width. :param wait_for_result: if True, this method will block till response is recieved from server :type wait_for_result: bool :return: success :rtype: bool """ self._caller = "home_joints" goal = HomingGoal() self._homing_action_client.send_goal(goal, done_cb =self._done_cb, active_cb = self._active_cb, feedback_cb = self._feedback_cb) if wait_for_result: result = self._homing_action_client.wait_for_result(rospy.Duration(15.)) return result return True def open(self): """ Open gripper to max possible width. :return: True if command was successful, False otherwise. :rtype: bool """ self._caller = "open gripper" return self.move_joints(0.2) def close(self): """ close gripper to till collision is detected. .. note:: This method is known to have a bug where it sometimes opens the gripper when it is already closed. Use :py:meth:`close` with argument 0 (zero) if you want to close it, or use :py:meth:`grasp` wherever possible. .. note:: This is not exactly doing what it should. The behaviour is faked by catching the error thrown when trying to grasp a very small object with a very small force. Since the gripper will actually hit the object before it reaches the commanded width, we catch the feedback and send the gripper stop command to stop it where it is. :return: True if command was successful, False otherwise. :rtype: bool """ def cb( _, result): if not result.success: self.stop_action() self._caller = "close gripper" return self.grasp(0.0, 0.1, cb = cb) def calibrate(self): return self.home_joints(wait_for_result = True) def move_joints(self, width, speed = None, wait_for_result = True): """ Moves the gripper fingers to a specified width. :param width: Intended opening width. [m] :param speed: Closing speed. [m/s] :param wait_for_result: if True, this method will block till response is recieved from server :type width: float :type speed: float :type wait_for_result: bool :return: True if command was successful, False otherwise. :rtype: bool """ self._caller = "move_joints" goal = MoveGoal() if not speed: speed = self._gripper_speed goal.width = width goal.speed = speed self._move_action_client.send_goal(goal, done_cb =self._done_cb, active_cb = self._active_cb, feedback_cb = self._feedback_cb) if wait_for_result: result = self._move_action_client.wait_for_result(rospy.Duration(15.)) return result return True def stop_action(self): """ Stops a currently running gripper move or grasp. :return: True if command was successful, False otherwise. :rtype: bool """ self._caller = "stop_action" goal = StopGoal() self._stop_action_client.send_goal(goal, done_cb =self._done_cb, active_cb = self._active_cb, feedback_cb = self._feedback_cb) result = self._stop_action_client.wait_for_result(rospy.Duration(15.)) return result def grasp(self, width, force, speed = None, epsilon_inner = 0.005, epsilon_outer = 0.005,wait_for_result = True, cb = None): """ Grasps an object. An object is considered grasped if the distance :math:`d` between the gripper fingers satisfies :math:`(width - epsilon\_inner) < d < (width + epsilon\_outer)`. :param width: Size of the object to grasp. [m] :param speed: Closing speed. [m/s] :param force: Grasping force. [N] :param epsilon_inner: Maximum tolerated deviation when the actual grasped width is smaller than the commanded grasp width. :param epsilon_outer: Maximum tolerated deviation when the actual grasped width is wider than the commanded grasp width. :param cb: Optional callback function to use when the service call is done :type width: float :type speed: float :type force: float :type epsilon_inner: float :type epsilon_outer: float :return: True if an object has been grasped, false otherwise. :rtype: bool """ self._caller = "grasp_action" if not speed: speed = self._gripper_speed goal = GraspGoal() goal.width = width goal.speed = speed goal.force = force goal.epsilon = GraspEpsilon(inner = epsilon_inner, outer = epsilon_outer) if not cb: cb = self._done_cb self._grasp_action_client.send_goal(goal, done_cb = cb, active_cb = self._active_cb, feedback_cb = self._feedback_cb) if wait_for_result: result = self._grasp_action_client.wait_for_result(rospy.Duration(15.)) return result return True ================================================ FILE: franka_interface/src/franka_interface/robot_enable.py ================================================ # /*************************************************************************** # # @package: franka_interface # @metapackage: franka_ros_interface # @author: Saif Sidhik # # **************************************************************************/ # /*************************************************************************** # Copyright (c) 2019-2021, Saif Sidhik # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. # You may obtain a copy of the License at # http://www.apache.org/licenses/LICENSE-2.0 # Unless required by applicable law or agreed to in writing, software # distributed under the License is distributed on an "AS IS" BASIS, # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. # See the License for the specific language governing permissions and # limitations under the License. # **************************************************************************/ """ :info: Wrapper class for enabling and resetting robot state. """ import rospy from threading import Lock from franka_msgs.msg import ErrorRecoveryActionGoal from franka_core_msgs.msg import RobotState import franka_dataflow from .robot_params import RobotParams class RobotEnable(object): """ Class RobotEnable - simple control/status wrapper around robot state enable() - enable all joints disable() - disable all joints reset() - reset all joints, reset all jrcp faults, disable the robot stop() - stop the robot, similar to hitting the e-stop button :param robot_params: A RobotParams instance (optional) :type robot_params: RobotParams """ param_lock = Lock() def __init__(self, robot_params = None): """ """ if robot_params: self._params = robot_params else: self._params = RobotParams() self._ns = self._params.get_base_namespace() self._enabled = None state_topic = '{}/custom_franka_state_controller/robot_state'.format(self._ns) self._state_sub = rospy.Subscriber(state_topic, RobotState, self._state_callback ) franka_dataflow.wait_for( lambda: not self._enabled is None, timeout=5.0, timeout_msg=("Failed to get robot state on %s" % (state_topic,)), ) def _state_callback(self, msg): self._enabled = (msg.robot_mode != 4) def is_enabled(self): """ Return status of robot :return: True if enabled, False otherwise :rtype: bool """ return self._enabled def _toggle_enabled(self, status): pub = rospy.Publisher('{}/franka_control/error_recovery/goal'.format(self._ns), ErrorRecoveryActionGoal, queue_size=10) if self._enabled == status: rospy.loginfo("Robot is already %s"%self.state()) franka_dataflow.wait_for( test=lambda: self._enabled == status, timeout=5.0, timeout_msg=("Failed to %sable robot" % ('en' if status else 'dis',)), body=lambda: pub.publish(ErrorRecoveryActionGoal()), ) rospy.loginfo("Robot %s", ('Enabled' if status else 'Disabled')) def state(self): """ Returns the last known robot state. :rtype: str :return: "Enabled"/"Disabled" """ return "%sabled"%('en' if self._enabled else 'dis',) def enable(self): """ Enable all joints """ if not self._enabled: rospy.loginfo("Robot Stopped: Attempting Reset...") self._toggle_enabled(True) def disable(self): """ Disable all joints """ self._toggle_enabled(False) ================================================ FILE: franka_interface/src/franka_interface/robot_params.py ================================================ # /*************************************************************************** # # @package: franka_interface # @metapackage: franka_ros_interface # @author: Saif Sidhik # # **************************************************************************/ # /*************************************************************************** # Copyright (c) 2019-2021, Saif Sidhik # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. # You may obtain a copy of the License at # http://www.apache.org/licenses/LICENSE-2.0 # Unless required by applicable law or agreed to in writing, software # distributed under the License is distributed on an "AS IS" BASIS, # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. # See the License for the specific language governing permissions and # limitations under the License. # **************************************************************************/ """ :info: Interface class for collecting and storing necessary parameters from ROS parameter server. """ import sys import rospy import socket from franka_core_msgs.msg import JointLimits def _log_networking_error(): print ("Failed to connect to the ROS parameter server!\n" "Please check to make sure your ROS networking is " "properly configured:\n") sys.exit() class RobotParams(object): """ Interface class for essential ROS parameters on the Franka Emika Panda robot. """ def __init__(self): if self._robot_in_simulation(): self._in_sim = True self._ns = "/panda_simulator" rospy.loginfo("Robot control running in Simulation Mode!") self._robot_ip="sim" else: self._ns = "/franka_ros_interface" self._in_sim = False self._robot_ip = self.get_robot_ip() self._robot_name = self.get_robot_name() def get_base_namespace(self): return self._ns def _robot_in_simulation(self): sim = False try: sim = rospy.get_param("/SIMULATOR_") except KeyError: sim = False except (socket.error, socket.gaierror): _log_networking_error() return sim def get_neutral_pose(self): """ Get neutral pose joint positions from parameter server (/robot_config/neutral_pose) :return: Joint positions of the robot as defined in parameter server. :rtype: [type] """ try: neutral_pose = rospy.get_param("/robot_config/neutral_pose") except KeyError: rospy.logerr("RobotParam: robot_ip cannot detect neutral joint pos." " under param /franka_control/neutral_pose") except (socket.error, socket.gaierror): _log_networking_error() return neutral_pose def get_robot_ip(self): robot_ip = None try: robot_ip = rospy.get_param("/franka_control/robot_ip") except KeyError: try: robot_ip = rospy.get_param("/robot_config/robot_ip") except KeyError: rospy.logerr("RobotParam: robot_ip cannot detect robot ip." " under param /robot_ip") except (socket.error, socket.gaierror): _log_networking_error() return robot_ip def get_joint_names(self): """ Return the names of the joints for the robot from ROS parameter server (/robot_config/joint_names). :rtype: [str] :return: ordered list of joint names from proximal to distal (i.e. shoulder to wrist). joint names for limb """ joint_names = list() try: joint_names = rospy.get_param("/robot_config/joint_names") except KeyError: rospy.logerr("RobotParam: get_joint_names cannot detect joint_names for arm") except (socket.error, socket.gaierror): _log_networking_error() return joint_names def get_gripper_joint_names(self): """ Return the names of the joints for the gripper from ROS parameter server (/gripper_config/joint_names). :rtype: [str] :return: ordered list of joint names """ joint_names = list() try: joint_names = rospy.get_param("/gripper_config/joint_names") except KeyError: rospy.loginfo("RobotParam: get_gripper_joint_names cannot detect joint_names for gripper.") return None except (socket.error, socket.gaierror): _log_networking_error() return joint_names def get_robot_name(self): """ Return the name of class of robot from ROS parameter server. (/robot_config/arm_id) :rtype: str :return: name of the robot """ robot_name = None try: robot_name = rospy.get_param("/robot_config/arm_id") except KeyError: rospy.logerr("RobotParam: get_robot_name cannot detect robot name" " under param /robot_config/arm_id") except (socket.error, socket.gaierror): _log_networking_error() return robot_name def get_joint_limits(self): """ Get joint limits as defined in ROS parameter server (/robot_config/joint_config/joint_velocity_limit) :return: Joint limits for each joints :rtype: franka_core_msgs.msg.JointLimits """ lims = JointLimits() lims.joint_names = self.get_joint_names() try: vel_lim = rospy.get_param("/robot_config/joint_config/joint_velocity_limit") except KeyError: rospy.logerr("RobotParam: get_joint_limits cannot detect robot joint velocity limits" " under param /robot_config/joint_config/joint_velocity_limit") except (socket.error, socket.gaierror): _log_networking_error() try: pos_min_lim = rospy.get_param("/robot_config/joint_config/joint_position_limit/lower") except KeyError: rospy.logerr("RobotParam: get_joint_limits cannot detect robot joint position lower limits" " under param /robot_config/joint_config/joint_position_limit/lower") except (socket.error, socket.gaierror): _log_networking_error() try: pos_max_lim = rospy.get_param("/robot_config/joint_config/joint_position_limit/upper") except KeyError: rospy.logerr("RobotParam: get_joint_limits cannot detect robot joint position upper limits" " under param /robot_config/joint_config/joint_position_limit/upper") except (socket.error, socket.gaierror): _log_networking_error() try: eff_lim = rospy.get_param("/robot_config/joint_config/joint_effort_limit") except KeyError: rospy.logerr("RobotParam: get_joint_limits cannot detect robot joint torque limits" " under param /robot_config/joint_config/joint_effort_limit") except (socket.error, socket.gaierror): _log_networking_error() try: acc_lim = rospy.get_param("/robot_config/joint_config/joint_acceleration_limit") except KeyError: rospy.logerr("RobotParam: get_joint_limits cannot detect robot joint acceleration limits" " under param /robot_config/joint_config/joint_acceleration_limit") except (socket.error, socket.gaierror): _log_networking_error() for i in range(len(lims.joint_names)): lims.position_upper.append(pos_max_lim[lims.joint_names[i]]) lims.position_lower.append(pos_min_lim[lims.joint_names[i]]) lims.velocity.append(vel_lim[lims.joint_names[i]]) lims.accel.append(acc_lim[lims.joint_names[i]]) lims.effort.append(eff_lim[lims.joint_names[i]]) return lims if __name__ == '__main__': rp = RobotParams() # print rp.__dict__ print(rp.get_robot_ip()) print(rp.get_robot_name()) print(rp.get_joint_names()) ================================================ FILE: franka_interface/src/motion_controller_interface.cpp ================================================ /*************************************************************************** * Adapted from arm_controller_interface.cpp (sawyer_simulator package) * * @package: franka_interface * @metapackage: franka_ros_interface * @author: Saif Sidhik * **************************************************************************/ /*************************************************************************** * Copyright (c) 2019-2021, Saif Sidhik * Copyright (c) 2013-2018, Rethink Robotics Inc. * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. **************************************************************************/ #include #include namespace franka_interface { void MotionControllerInterface::init(ros::NodeHandle& nh, boost::shared_ptr controller_manager) { current_mode_ = -1; if (!nh.getParam("/controllers_config/position_controller", position_controller_name_)) { position_controller_name_ = "position_joint_position_controller"; } if (!nh.getParam("/controllers_config/torque_controller", torque_controller_name_)) { torque_controller_name_ = "effort_joint_torque_controller"; } if (!nh.getParam("/controllers_config/impedance_controller", impedance_controller_name_)) { impedance_controller_name_ = "effort_joint_impedance_controller"; } if (!nh.getParam("/controllers_config/velocity_controller", velocity_controller_name_)) { velocity_controller_name_ = "velocity_joint_velocity_controller"; } if (!nh.getParam("/controllers_config/trajectory_controller", trajectory_controller_name_)) { trajectory_controller_name_ = "position_joint_trajectory_controller"; } if (!nh.getParam("/controllers_config/default_controller", default_controller_name_)) { default_controller_name_ = "position_joint_trajectory_controller"; } current_controller_name_ = default_controller_name_; all_controllers_.clear(); all_controllers_.push_back(position_controller_name_); all_controllers_.push_back(torque_controller_name_); all_controllers_.push_back(impedance_controller_name_); all_controllers_.push_back(velocity_controller_name_); all_controllers_.push_back(trajectory_controller_name_); bool default_defined = false; for (size_t i = 0; i < all_controllers_.size(); ++i){ if (all_controllers_[i] == default_controller_name_){ default_defined = true; break; } } controller_name_to_mode_map_[position_controller_name_] = franka_core_msgs::JointCommand::POSITION_MODE; controller_name_to_mode_map_[torque_controller_name_] = franka_core_msgs::JointCommand::TORQUE_MODE; controller_name_to_mode_map_[impedance_controller_name_] = franka_core_msgs::JointCommand::IMPEDANCE_MODE; controller_name_to_mode_map_[velocity_controller_name_] = franka_core_msgs::JointCommand::VELOCITY_MODE; controller_name_to_mode_map_[trajectory_controller_name_] = -1; if (! default_defined){ ROS_ERROR_STREAM_NAMED("MotionControllerInterface", "Default controller not present in the provided controllers!"); } controller_manager_ = controller_manager; joint_command_sub_ = nh.subscribe("/franka_ros_interface/motion_controller/arm/joint_commands", 1, &MotionControllerInterface::jointCommandCallback, this); // Command Timeout joint_command_timeout_sub_ = nh.subscribe("/franka_ros_interface/motion_controller/arm/joint_command_timeout", 1, &MotionControllerInterface::jointCommandTimeoutCallback, this); double command_timeout_default; nh.param("/controllers_config/command_timeout", command_timeout_default, 0.2); auto p_cmd_timeout_length = std::make_shared(std::min(1.0, std::max(0.0, command_timeout_default))); box_timeout_length_.set(p_cmd_timeout_length); ROS_INFO_STREAM("MotionControllerInterface Initialised"); // Update at 100Hz cmd_timeout_timer_ = nh.createTimer(100, &MotionControllerInterface::commandTimeoutCheck, this); } void MotionControllerInterface::commandTimeoutCheck(const ros::TimerEvent& e) { // lock out other thread(s) which are getting called back via ros. std::lock_guard guard(mtx_); // Check Command Timeout std::shared_ptr p_timeout_length; box_timeout_length_.get(p_timeout_length); std::shared_ptr p_cmd_msg_time; box_cmd_timeout_.get(p_cmd_msg_time); bool command_timeout = (p_cmd_msg_time && p_timeout_length && ((ros::Time::now() - *p_cmd_msg_time.get()) > (*p_timeout_length.get()))); if(command_timeout && (current_controller_name_ != default_controller_name_)) { // Timeout violated, force robot back to Default Controller Mode ROS_WARN_STREAM("MotionControllerInterface: Command timeout violated: Switching to Default control mode." << default_controller_name_); switchToDefaultController(); } } bool MotionControllerInterface::switchToDefaultController() { std::vector start_controllers; std::vector stop_controllers; for (size_t i = 0; i < all_controllers_.size(); ++i) { if (all_controllers_[i] == default_controller_name_) start_controllers.push_back(all_controllers_[i]); else stop_controllers.push_back(all_controllers_[i]); } if (!controller_manager_->switchController(start_controllers, stop_controllers, controller_manager_msgs::SwitchController::Request::BEST_EFFORT)) { ROS_ERROR_STREAM_NAMED("MotionControllerInterface", "Failed to switch controllers"); return false; } current_controller_name_ = start_controllers[0]; current_mode_ = controller_name_to_mode_map_[current_controller_name_]; ROS_INFO_STREAM("MotionControllerInterface: Controller " << start_controllers[0] << " started; Controllers " << stop_controllers[0] << ", " << stop_controllers[1] << ", " << stop_controllers[2] << ", " << stop_controllers[3] << " stopped."); return true; } void MotionControllerInterface::jointCommandTimeoutCallback(const std_msgs::Float64 msg) { ROS_INFO_STREAM("MotionControllerInterface: Joint command timeout: " << msg.data); auto p_cmd_timeout_length = std::make_shared( std::min(1.0, std::max(0.0, double(msg.data)))); box_timeout_length_.set(p_cmd_timeout_length); } bool MotionControllerInterface::switchControllers(int control_mode) { std::vector start_controllers; std::vector stop_controllers; if(current_mode_ != control_mode) { switch (control_mode) { case franka_core_msgs::JointCommand::POSITION_MODE: start_controllers.push_back(position_controller_name_); stop_controllers.push_back(impedance_controller_name_); stop_controllers.push_back(torque_controller_name_); stop_controllers.push_back(velocity_controller_name_); stop_controllers.push_back(trajectory_controller_name_); break; case franka_core_msgs::JointCommand::IMPEDANCE_MODE: start_controllers.push_back(impedance_controller_name_); stop_controllers.push_back(position_controller_name_); stop_controllers.push_back(torque_controller_name_); stop_controllers.push_back(velocity_controller_name_); stop_controllers.push_back(trajectory_controller_name_); break; case franka_core_msgs::JointCommand::TORQUE_MODE: start_controllers.push_back(torque_controller_name_); stop_controllers.push_back(position_controller_name_); stop_controllers.push_back(impedance_controller_name_); stop_controllers.push_back(velocity_controller_name_); stop_controllers.push_back(trajectory_controller_name_); break; case franka_core_msgs::JointCommand::VELOCITY_MODE: start_controllers.push_back(velocity_controller_name_); stop_controllers.push_back(position_controller_name_); stop_controllers.push_back(impedance_controller_name_); stop_controllers.push_back(torque_controller_name_); stop_controllers.push_back(trajectory_controller_name_); break; default: ROS_ERROR_STREAM_NAMED("MotionControllerInterface", "Unknown JointCommand mode " << control_mode << ". Ignoring command."); return false; } if (!controller_manager_->switchController(start_controllers, stop_controllers, controller_manager_msgs::SwitchController::Request::BEST_EFFORT)) { ROS_ERROR_STREAM_NAMED("MotionControllerInterface", "Failed to switch controllers"); return false; } current_mode_ = control_mode; current_controller_name_ = start_controllers[0]; ROS_INFO_STREAM("MotionControllerInterface: Controller " << start_controllers[0] << " started; Controllers " << stop_controllers[0] << ", " << stop_controllers[1] << ", " << stop_controllers[2] << ", " << stop_controllers[3] << " stopped."); } return true; } void MotionControllerInterface::jointCommandCallback(const franka_core_msgs::JointCommandConstPtr& msg) { // lock out other thread(s) which are getting called back via ros. std::lock_guard guard(mtx_); if(switchControllers(msg->mode)) { auto p_cmd_msg_time = std::make_shared(ros::Time::now()); box_cmd_timeout_.set(p_cmd_msg_time); } } } ================================================ FILE: franka_interface/src/robot_state_controller.cpp ================================================ /*************************************************************************** * Adapted from robot_state_publisher.cpp (franka_ros package) * * @package: panda_gazebo * @metapackage: panda_simulator * @author: Saif Sidhik * **************************************************************************/ /*************************************************************************** * Copyright (c) 2019-2021, Saif Sidhik. * Copyright (c) 2017 Franka Emika GmbH * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. **************************************************************************/ #include #include #include #include #include #include #include #include #include #include #include #include #include namespace { tf::Transform convertArrayToTf(const std::array& transform) { tf::Matrix3x3 rotation(transform[0], transform[4], transform[8], transform[1], transform[5], transform[9], transform[2], transform[6], transform[10]); tf::Vector3 translation(transform[12], transform[13], transform[14]); return tf::Transform(rotation, translation); } franka_msgs::Errors errorsToMessage(const franka::Errors& error) { franka_msgs::Errors message; message.joint_position_limits_violation = static_cast( error.joint_position_limits_violation); message.cartesian_position_limits_violation = static_cast( error.cartesian_position_limits_violation); message.self_collision_avoidance_violation = static_cast( error.self_collision_avoidance_violation); message.joint_velocity_violation = static_cast(error.joint_velocity_violation); message.cartesian_velocity_violation = static_cast( error.cartesian_velocity_violation); message.force_control_safety_violation = static_cast( error.force_control_safety_violation); message.joint_reflex = static_cast(error.joint_reflex); message.cartesian_reflex = static_cast(error.cartesian_reflex); message.max_goal_pose_deviation_violation = static_cast( error.max_goal_pose_deviation_violation); message.max_path_pose_deviation_violation = static_cast( error.max_path_pose_deviation_violation); message.cartesian_velocity_profile_safety_violation = static_cast( error.cartesian_velocity_profile_safety_violation); message.joint_position_motion_generator_start_pose_invalid = static_cast( error.joint_position_motion_generator_start_pose_invalid); message.joint_motion_generator_position_limits_violation = static_cast( error.joint_motion_generator_position_limits_violation); message.joint_motion_generator_velocity_limits_violation = static_cast( error.joint_motion_generator_velocity_limits_violation); message.joint_motion_generator_velocity_discontinuity = static_cast( error.joint_motion_generator_velocity_discontinuity); message.joint_motion_generator_acceleration_discontinuity = static_cast( error.joint_motion_generator_acceleration_discontinuity); message.cartesian_position_motion_generator_start_pose_invalid = static_cast( error.cartesian_position_motion_generator_start_pose_invalid); message.cartesian_motion_generator_elbow_limit_violation = static_cast( error.cartesian_motion_generator_elbow_limit_violation); message.cartesian_motion_generator_velocity_limits_violation = static_cast( error.cartesian_motion_generator_velocity_limits_violation); message.cartesian_motion_generator_velocity_discontinuity = static_cast( error.cartesian_motion_generator_velocity_discontinuity); message.cartesian_motion_generator_acceleration_discontinuity = static_cast( error.cartesian_motion_generator_acceleration_discontinuity); message.cartesian_motion_generator_elbow_sign_inconsistent = static_cast( error.cartesian_motion_generator_elbow_sign_inconsistent); message.cartesian_motion_generator_start_elbow_invalid = static_cast( error.cartesian_motion_generator_start_elbow_invalid); message.cartesian_motion_generator_joint_position_limits_violation = static_cast( error.cartesian_motion_generator_joint_position_limits_violation); message.cartesian_motion_generator_joint_velocity_limits_violation = static_cast( error.cartesian_motion_generator_joint_velocity_limits_violation); message.cartesian_motion_generator_joint_velocity_discontinuity = static_cast( error.cartesian_motion_generator_joint_velocity_discontinuity); message.cartesian_motion_generator_joint_acceleration_discontinuity = static_cast( error.cartesian_motion_generator_joint_acceleration_discontinuity); message.cartesian_position_motion_generator_invalid_frame = static_cast( error.cartesian_position_motion_generator_invalid_frame); message.force_controller_desired_force_tolerance_violation = static_cast( error.force_controller_desired_force_tolerance_violation); message.controller_torque_discontinuity = static_cast( error.controller_torque_discontinuity); message.start_elbow_sign_inconsistent = static_cast( error.start_elbow_sign_inconsistent); message.communication_constraints_violation = static_cast( error.communication_constraints_violation); message.power_limit_violation = static_cast(error.power_limit_violation); message.joint_p2p_insufficient_torque_for_planning = static_cast( error.joint_p2p_insufficient_torque_for_planning); message.tau_j_range_violation = static_cast(error.tau_j_range_violation); message.instability_detected = static_cast(error.instability_detected); return message; } } // anonymous namespace namespace franka_interface { bool CustomFrankaStateController::init(hardware_interface::RobotHW* robot_hardware, ros::NodeHandle& root_node_handle, ros::NodeHandle& controller_node_handle) { franka_state_interface_ = robot_hardware->get(); if (franka_state_interface_ == nullptr) { ROS_ERROR("CustomFrankaStateController: Could not get Franka state interface from hardware"); return false; } if (!controller_node_handle.getParam("arm_id", arm_id_)) { ROS_ERROR("CustomFrankaStateController: Could not get parameter arm_id"); return false; } double publish_rate(500.0); if (!controller_node_handle.getParam("publish_rate", publish_rate)) { ROS_INFO_STREAM("CustomFrankaStateController: Did not find publish_rate. Using default " << publish_rate << " [Hz]."); } trigger_publish_ = franka_hw::TriggerRate(publish_rate); if (!controller_node_handle.getParam("joint_names", joint_names_) || joint_names_.size() != robot_state_.q.size()) { ROS_ERROR( "CustomFrankaStateController: Invalid or no joint_names parameters provided, aborting " "controller init!"); return false; } try { franka_state_handle_ = std::make_unique( franka_state_interface_->getHandle(arm_id_ + "_robot")); } catch (const hardware_interface::HardwareInterfaceException& ex) { ROS_ERROR_STREAM("CustomFrankaStateController: Exception getting franka state handle: " << ex.what()); return false; } auto* model_interface = robot_hardware->get(); if (model_interface == nullptr) { ROS_ERROR_STREAM( "CustomFrankaStateController: Error getting model interface from hardware"); return false; } try { model_handle_ = std::make_unique( model_interface->getHandle(arm_id_ + "_model")); } catch (hardware_interface::HardwareInterfaceException& ex) { ROS_ERROR_STREAM( "CustomFrankaStateController: Exception getting model handle from interface: " << ex.what()); return false; } publisher_transforms_.init(root_node_handle, "/tf", 1); publisher_franka_state_.init(controller_node_handle, "robot_state", 1); publisher_joint_states_.init(controller_node_handle, "joint_states", 1); publisher_joint_states_desired_.init(controller_node_handle, "joint_states_desired", 1); publisher_tip_state_.init(controller_node_handle, "tip_state", 1); { std::lock_guard > lock( publisher_joint_states_); publisher_joint_states_.msg_.name.resize(joint_names_.size()); publisher_joint_states_.msg_.position.resize(robot_state_.q.size()); publisher_joint_states_.msg_.velocity.resize(robot_state_.dq.size()); publisher_joint_states_.msg_.effort.resize(robot_state_.tau_J.size()); } { std::lock_guard > lock( publisher_joint_states_desired_); publisher_joint_states_desired_.msg_.name.resize(joint_names_.size()); publisher_joint_states_desired_.msg_.position.resize(robot_state_.q_d.size()); publisher_joint_states_desired_.msg_.velocity.resize(robot_state_.dq_d.size()); publisher_joint_states_desired_.msg_.effort.resize(robot_state_.tau_J_d.size()); } { std::lock_guard > lock( publisher_transforms_); publisher_transforms_.msg_.transforms.resize(3); tf::Quaternion quaternion(0.0, 0.0, 0.0, 1.0); tf::Vector3 translation(0.0, 0.0, 0.0); tf::Transform transform(quaternion, translation); tf::StampedTransform stamped_transform(transform, ros::Time::now(), arm_id_ + "_link8", arm_id_ + "_NE"); geometry_msgs::TransformStamped transform_message; transformStampedTFToMsg(stamped_transform, transform_message); publisher_transforms_.msg_.transforms[0] = transform_message; translation = tf::Vector3(0.0, 0.0, 0.0); transform = tf::Transform(quaternion, translation); stamped_transform = tf::StampedTransform(transform, ros::Time::now(), arm_id_ + "_NE", arm_id_ + "_EE"); transformStampedTFToMsg(stamped_transform, transform_message); publisher_transforms_.msg_.transforms[1] = transform_message; translation = tf::Vector3(0.0, 0.0, 0.0); transform = tf::Transform(quaternion, translation); stamped_transform = tf::StampedTransform(transform, ros::Time::now(), arm_id_ + "_EE", arm_id_ + "_K"); transformStampedTFToMsg(stamped_transform, transform_message); publisher_transforms_.msg_.transforms[2] = transform_message; } { std::lock_guard > lock( publisher_tip_state_); publisher_tip_state_.msg_.O_F_ext_hat_K.header.frame_id = arm_id_ + "_link0"; publisher_tip_state_.msg_.O_F_ext_hat_K.wrench.force.x = 0.0; publisher_tip_state_.msg_.O_F_ext_hat_K.wrench.force.y = 0.0; publisher_tip_state_.msg_.O_F_ext_hat_K.wrench.force.z = 0.0; publisher_tip_state_.msg_.O_F_ext_hat_K.wrench.torque.x = 0.0; publisher_tip_state_.msg_.O_F_ext_hat_K.wrench.torque.y = 0.0; publisher_tip_state_.msg_.O_F_ext_hat_K.wrench.torque.z = 0.0; publisher_tip_state_.msg_.K_F_ext_hat_K.header.frame_id = arm_id_ + "_K"; publisher_tip_state_.msg_.K_F_ext_hat_K.wrench.force.x = 0.0; publisher_tip_state_.msg_.K_F_ext_hat_K.wrench.force.y = 0.0; publisher_tip_state_.msg_.K_F_ext_hat_K.wrench.force.z = 0.0; publisher_tip_state_.msg_.K_F_ext_hat_K.wrench.torque.x = 0.0; publisher_tip_state_.msg_.K_F_ext_hat_K.wrench.torque.y = 0.0; publisher_tip_state_.msg_.K_F_ext_hat_K.wrench.torque.z = 0.0; } return true; } void CustomFrankaStateController::update(const ros::Time& time, const ros::Duration& /* period */) { if (trigger_publish_()) { robot_state_ = franka_state_handle_->getRobotState(); publishFrankaState(time); publishTransforms(time); publishEndPointState(time); publishJointStates(time); sequence_number_++; } } void CustomFrankaStateController::publishFrankaState(const ros::Time& time) { std::array coriolis = model_handle_->getCoriolis(); std::array gravity = model_handle_->getGravity(); std::array mass_matrix = model_handle_->getMass(); std::array O_Jac_EE = model_handle_->getZeroJacobian(franka::Frame::kEndEffector); Eigen::Map> jacobian(O_Jac_EE.data()); Eigen::Map> dq(robot_state_.dq.data()); // jacobian * dq Eigen::Matrix ee_vel = jacobian * dq; if (publisher_franka_state_.trylock()) { static_assert( sizeof(robot_state_.cartesian_collision) == sizeof(robot_state_.cartesian_contact), "Robot state Cartesian members do not have same size"); static_assert( sizeof(robot_state_.cartesian_collision) == sizeof(robot_state_.O_dP_EE_c), "Robot state Cartesian members do not have same size"); static_assert( sizeof(robot_state_.cartesian_collision) == sizeof(robot_state_.O_dP_EE_d), "Robot state Cartesian members do not have same size"); static_assert( sizeof(robot_state_.cartesian_collision) == sizeof(robot_state_.O_ddP_EE_c), "Robot state Cartesian members do not have same size"); for (size_t i = 0; i < robot_state_.cartesian_collision.size(); i++) { publisher_franka_state_.msg_.cartesian_collision[i] = robot_state_.cartesian_collision[i]; publisher_franka_state_.msg_.cartesian_contact[i] = robot_state_.cartesian_contact[i]; publisher_franka_state_.msg_.O_dP_EE[i] = ee_vel(i,0); } static_assert(sizeof(robot_state_.q) == sizeof(robot_state_.q_d), "Robot state joint members do not have same size"); static_assert(sizeof(robot_state_.q) == sizeof(robot_state_.dq_d), "Robot state joint members do not have same size"); static_assert(sizeof(robot_state_.q) == sizeof(robot_state_.dtau_J), "Robot state joint members do not have same size"); static_assert(sizeof(robot_state_.q) == sizeof(robot_state_.tau_J_d), "Robot state joint members do not have same size"); static_assert(sizeof(robot_state_.q) == sizeof(robot_state_.joint_collision), "Robot state joint members do not have same size"); static_assert(sizeof(robot_state_.q) == sizeof(robot_state_.joint_contact), "Robot state joint members do not have same size"); static_assert(sizeof(robot_state_.q) == sizeof(robot_state_.tau_ext_hat_filtered), "Robot state joint members do not have same size"); for (size_t i = 0; i < robot_state_.q.size(); i++) { publisher_franka_state_.msg_.q_d[i] = robot_state_.q_d[i]; publisher_franka_state_.msg_.dq_d[i] = robot_state_.dq_d[i]; publisher_franka_state_.msg_.dtau_J[i] = robot_state_.dtau_J[i]; publisher_franka_state_.msg_.tau_J_d[i] = robot_state_.tau_J_d[i]; publisher_franka_state_.msg_.joint_collision[i] = robot_state_.joint_collision[i]; publisher_franka_state_.msg_.joint_contact[i] = robot_state_.joint_contact[i]; publisher_franka_state_.msg_.tau_ext_hat_filtered[i] = robot_state_.tau_ext_hat_filtered[i]; publisher_franka_state_.msg_.gravity[i] = gravity[i]; publisher_franka_state_.msg_.coriolis[i] = coriolis[i]; } static_assert(sizeof(robot_state_.O_T_EE) == sizeof(robot_state_.F_T_EE), "Robot state transforms do not have same size"); static_assert(sizeof(robot_state_.O_T_EE) == sizeof(robot_state_.EE_T_K), "Robot state transforms do not have same size"); static_assert(sizeof(robot_state_.O_T_EE) == sizeof(robot_state_.O_T_EE_d), "Robot state transforms do not have same size"); static_assert(sizeof(robot_state_.NE_T_EE) == sizeof(robot_state_.O_T_EE), "Robot state transforms do not have same size"); for (size_t i = 0; i < robot_state_.O_T_EE.size(); i++) { publisher_franka_state_.msg_.F_T_EE[i] = robot_state_.F_T_EE[i]; publisher_franka_state_.msg_.EE_T_K[i] = robot_state_.EE_T_K[i]; publisher_franka_state_.msg_.O_T_EE_d[i] = robot_state_.O_T_EE_d[i]; publisher_franka_state_.msg_.F_T_NE[i] = robot_state_.F_T_NE[i]; publisher_franka_state_.msg_.NE_T_EE[i] = robot_state_.NE_T_EE[i]; } publisher_franka_state_.msg_.m_ee = robot_state_.m_ee; publisher_franka_state_.msg_.m_load = robot_state_.m_load; publisher_franka_state_.msg_.m_total = robot_state_.m_total; for (size_t i = 0; i < robot_state_.I_load.size(); i++) { publisher_franka_state_.msg_.I_ee[i] = robot_state_.I_ee[i]; publisher_franka_state_.msg_.I_load[i] = robot_state_.I_load[i]; publisher_franka_state_.msg_.I_total[i] = robot_state_.I_total[i]; } for (size_t i = 0; i < robot_state_.F_x_Cload.size(); i++) { publisher_franka_state_.msg_.F_x_Cee[i] = robot_state_.F_x_Cee[i]; publisher_franka_state_.msg_.F_x_Cload[i] = robot_state_.F_x_Cload[i]; publisher_franka_state_.msg_.F_x_Ctotal[i] = robot_state_.F_x_Ctotal[i]; } for (size_t i = 0; i < mass_matrix.size(); i++) { publisher_franka_state_.msg_.mass_matrix[i] = mass_matrix[i]; } for (size_t i = 0; i < O_Jac_EE.size(); i++) { publisher_franka_state_.msg_.O_Jac_EE[i] = O_Jac_EE[i]; } publisher_franka_state_.msg_.time = robot_state_.time.toSec(); publisher_franka_state_.msg_.current_errors = errorsToMessage(robot_state_.current_errors); publisher_franka_state_.msg_.last_motion_errors = errorsToMessage(robot_state_.last_motion_errors); switch (robot_state_.robot_mode) { case franka::RobotMode::kOther: publisher_franka_state_.msg_.robot_mode = franka_core_msgs::RobotState::ROBOT_MODE_OTHER; break; case franka::RobotMode::kIdle: publisher_franka_state_.msg_.robot_mode = franka_core_msgs::RobotState::ROBOT_MODE_IDLE; break; case franka::RobotMode::kMove: publisher_franka_state_.msg_.robot_mode = franka_core_msgs::RobotState::ROBOT_MODE_MOVE; break; case franka::RobotMode::kGuiding: publisher_franka_state_.msg_.robot_mode = franka_core_msgs::RobotState::ROBOT_MODE_GUIDING; break; case franka::RobotMode::kReflex: publisher_franka_state_.msg_.robot_mode = franka_core_msgs::RobotState::ROBOT_MODE_REFLEX; break; case franka::RobotMode::kUserStopped: publisher_franka_state_.msg_.robot_mode = franka_core_msgs::RobotState::ROBOT_MODE_USER_STOPPED; break; case franka::RobotMode::kAutomaticErrorRecovery: publisher_franka_state_.msg_.robot_mode = franka_core_msgs::RobotState::ROBOT_MODE_AUTOMATIC_ERROR_RECOVERY; break; } publisher_franka_state_.msg_.header.seq = sequence_number_; publisher_franka_state_.msg_.header.stamp = time; publisher_franka_state_.unlockAndPublish(); } } void CustomFrankaStateController::publishJointStates(const ros::Time& time) { if (publisher_joint_states_.trylock()) { static_assert(sizeof(robot_state_.q) == sizeof(robot_state_.dq), "Robot state joint members do not have same size"); static_assert(sizeof(robot_state_.q) == sizeof(robot_state_.tau_J), "Robot state joint members do not have same size"); for (size_t i = 0; i < robot_state_.q.size(); i++) { publisher_joint_states_.msg_.name[i] = joint_names_[i]; publisher_joint_states_.msg_.position[i] = robot_state_.q[i]; publisher_joint_states_.msg_.velocity[i] = robot_state_.dq[i]; publisher_joint_states_.msg_.effort[i] = robot_state_.tau_J[i]; } publisher_joint_states_.msg_.header.stamp = time; publisher_joint_states_.msg_.header.seq = sequence_number_; publisher_joint_states_.unlockAndPublish(); } if (publisher_joint_states_desired_.trylock()) { static_assert(sizeof(robot_state_.q_d) == sizeof(robot_state_.dq_d), "Robot state joint members do not have same size"); static_assert(sizeof(robot_state_.q_d) == sizeof(robot_state_.tau_J_d), "Robot state joint members do not have same size"); for (size_t i = 0; i < robot_state_.q_d.size(); i++) { publisher_joint_states_desired_.msg_.name[i] = joint_names_[i]; publisher_joint_states_desired_.msg_.position[i] = robot_state_.q_d[i]; publisher_joint_states_desired_.msg_.velocity[i] = robot_state_.dq_d[i]; publisher_joint_states_desired_.msg_.effort[i] = robot_state_.tau_J_d[i]; } publisher_joint_states_desired_.msg_.header.stamp = time; publisher_joint_states_desired_.msg_.header.seq = sequence_number_; publisher_joint_states_desired_.unlockAndPublish(); } } void CustomFrankaStateController::publishTransforms(const ros::Time& time) { if (publisher_transforms_.trylock()) { tf::StampedTransform stamped_transform(convertArrayToTf(robot_state_.F_T_NE), time, arm_id_ + "_link8", arm_id_ + "_NE"); geometry_msgs::TransformStamped transform_message; transformStampedTFToMsg(stamped_transform, transform_message); publisher_transforms_.msg_.transforms[0] = transform_message; stamped_transform = tf::StampedTransform(convertArrayToTf(robot_state_.NE_T_EE), time, arm_id_ + "_NE", arm_id_ + "_EE"); transformStampedTFToMsg(stamped_transform, transform_message); publisher_transforms_.msg_.transforms[1] = transform_message; stamped_transform = tf::StampedTransform(convertArrayToTf(robot_state_.EE_T_K), time, arm_id_ + "_EE", arm_id_ + "_K"); transformStampedTFToMsg(stamped_transform, transform_message); publisher_transforms_.msg_.transforms[2] = transform_message; publisher_transforms_.unlockAndPublish(); } } void CustomFrankaStateController::publishEndPointState(const ros::Time& time) { if (publisher_tip_state_.trylock()) { for (size_t i = 0; i < robot_state_.O_T_EE.size(); i++) { publisher_tip_state_.msg_.O_T_EE[i] = robot_state_.O_T_EE[i]; } // for (size_t i = 0; i < robot_state_.O_dP_EE_c.size(); i++) { // publisher_tip_state_.msg_.O_dP_EE_c[i] = robot_state_.O_dP_EE_c[i]; // publisher_tip_state_.msg_.O_dP_EE_d[i] = robot_state_.O_dP_EE_d[i]; // publisher_tip_state_.msg_.O_ddP_EE_c[i] = robot_state_.O_ddP_EE_c[i]; // } publisher_tip_state_.msg_.O_F_ext_hat_K.header.frame_id = arm_id_ + "_link0"; publisher_tip_state_.msg_.O_F_ext_hat_K.header.stamp = time; publisher_tip_state_.msg_.O_F_ext_hat_K.wrench.force.x = robot_state_.O_F_ext_hat_K[0]; publisher_tip_state_.msg_.O_F_ext_hat_K.wrench.force.y = robot_state_.O_F_ext_hat_K[1]; publisher_tip_state_.msg_.O_F_ext_hat_K.wrench.force.z = robot_state_.O_F_ext_hat_K[2]; publisher_tip_state_.msg_.O_F_ext_hat_K.wrench.torque.x = robot_state_.O_F_ext_hat_K[3]; publisher_tip_state_.msg_.O_F_ext_hat_K.wrench.torque.y = robot_state_.O_F_ext_hat_K[4]; publisher_tip_state_.msg_.O_F_ext_hat_K.wrench.torque.z = robot_state_.O_F_ext_hat_K[5]; publisher_tip_state_.msg_.K_F_ext_hat_K.header.frame_id = arm_id_ + "_K"; publisher_tip_state_.msg_.K_F_ext_hat_K.header.stamp = time; publisher_tip_state_.msg_.K_F_ext_hat_K.wrench.force.x = robot_state_.K_F_ext_hat_K[0]; publisher_tip_state_.msg_.K_F_ext_hat_K.wrench.force.y = robot_state_.K_F_ext_hat_K[1]; publisher_tip_state_.msg_.K_F_ext_hat_K.wrench.force.z = robot_state_.K_F_ext_hat_K[2]; publisher_tip_state_.msg_.K_F_ext_hat_K.wrench.torque.x = robot_state_.K_F_ext_hat_K[3]; publisher_tip_state_.msg_.K_F_ext_hat_K.wrench.torque.y = robot_state_.K_F_ext_hat_K[4]; publisher_tip_state_.msg_.K_F_ext_hat_K.wrench.torque.z = robot_state_.K_F_ext_hat_K[5]; publisher_tip_state_.msg_.header.frame_id = arm_id_ + "_link0"; publisher_tip_state_.msg_.header.seq = sequence_number_; publisher_tip_state_.msg_.header.stamp = time; publisher_tip_state_.unlockAndPublish(); } } } // namespace franka_interface PLUGINLIB_EXPORT_CLASS(franka_interface::CustomFrankaStateController, controller_interface::ControllerBase) ================================================ FILE: franka_interface/state_controller_plugin.xml ================================================ A controller that publishes the complete robot state ================================================ FILE: franka_interface/tests/demo_interface.py ================================================ import rospy import numpy as np from franka_interface import ArmInterface """ :info: Demo showing ways to use API. Also shows how to move the robot using low-level controllers. WARNING: The robot will move slightly (small arc swinging motion side-to-side) till code is killed. """ if __name__ == '__main__': rospy.init_node("test_robot") r = ArmInterface() # create arm interface instance (see https://justagist.github.io/franka_ros_interface/DOC.html#arminterface for all available methods for ArmInterface() object) cm = r.get_controller_manager() # get controller manager instance associated with the robot (not required in most cases) 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) elapsed_time_ = rospy.Duration(0.0) period = rospy.Duration(0.005) r.move_to_neutral() # move robot to neutral pose initial_pose = r.joint_angles() # get current joint angles of the robot jac = r.zero_jacobian() # get end-effector jacobian count = 0 rate = rospy.Rate(1000) rospy.loginfo("Commanding...\n") joint_names = r.joint_names() vals = r.joint_angles() while not rospy.is_shutdown(): elapsed_time_ += period delta = 3.14 / 16.0 * (1 - np.cos(3.14 / 5.0 * elapsed_time_.to_sec())) * 0.2 for j in joint_names: if j == joint_names[4]: vals[j] = initial_pose[j] - delta else: vals[j] = initial_pose[j] + delta # r.set_joint_positions(vals) # for position control. Slightly jerky motion. r.set_joint_positions_velocities([vals[j] for j in joint_names], [0.0]*7) # for impedance control rate.sleep() ================================================ FILE: franka_interface/tests/demo_joint_positions_keyboard.py ================================================ #!/usr/bin/env python # /*************************************************************************** # # @package: franka_tools # @metapackage: franka_ros_interface # @author: Saif Sidhik # # **************************************************************************/ # /*************************************************************************** # Copyright (c) 2019-2021, Saif Sidhik # Copyright (c) 2013-2018, Rethink Robotics Inc. # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. # You may obtain a copy of the License at # http://www.apache.org/licenses/LICENSE-2.0 # Unless required by applicable law or agreed to in writing, software # distributed under the License is distributed on an "AS IS" BASIS, # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. # See the License for the specific language governing permissions and # limitations under the License. # **************************************************************************/ import rospy from future.utils import viewitems # for python2&3 efficient compatibility from franka_interface import ArmInterface, GripperInterface from franka_dataflow.getch import getch """ Franka ROS Interface Joint Position Example: Keyboard Control Use your dev machine's keyboard to control joint positions. Each key corresponds to increasing or decreasing the angle of a joint on the robot arm. The increasing and descreasing are represented by number key and letter key next to the number. :info: Demo showing low-level position control using Franka ROS Interface. Run the demo and press '?' on keyboard for command instructions. Disclaimer: This code is only for demonstration purpose, and does not show the ideal way to use the interface code. Warning: The robot will move according to the keyboard press. Also, note that the motion will be slightly jerky (small noise from the robot joints). This is because of the non-smooth commands sent to the robot. """ def map_keyboard(): """ Map keyboard keys to robot joint motion. Keybindings can be found when running the script. """ limb = ArmInterface() gripper = GripperInterface(ns = limb.get_robot_params().get_base_namespace()) has_gripper = gripper.exists joints = limb.joint_names() def set_j(limb, joint_name, delta): joint_command = limb.joint_angles() joint_command[joint_name] += delta limb.set_joint_positions(joint_command) # 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 def set_g(action): if has_gripper: if action == "close": gripper.close() elif action == "open": gripper.open() elif action == "calibrate": gripper.calibrate() bindings = { '1': (set_j, [limb, joints[0], 0.01], joints[0]+" increase"), 'q': (set_j, [limb, joints[0], -0.01], joints[0]+" decrease"), '2': (set_j, [limb, joints[1], 0.01], joints[1]+" increase"), 'w': (set_j, [limb, joints[1], -0.01], joints[1]+" decrease"), '3': (set_j, [limb, joints[2], 0.01], joints[2]+" increase"), 'e': (set_j, [limb, joints[2], -0.01], joints[2]+" decrease"), '4': (set_j, [limb, joints[3], 0.01], joints[3]+" increase"), 'r': (set_j, [limb, joints[3], -0.01], joints[3]+" decrease"), '5': (set_j, [limb, joints[4], 0.01], joints[4]+" increase"), 't': (set_j, [limb, joints[4], -0.01], joints[4]+" decrease"), '6': (set_j, [limb, joints[5], 0.01], joints[5]+" increase"), 'y': (set_j, [limb, joints[5], -0.01], joints[5]+" decrease"), '7': (set_j, [limb, joints[6], 0.01], joints[6]+" increase"), 'u': (set_j, [limb, joints[6], -0.01], joints[6]+" decrease") } if has_gripper: bindings.update({ '8': (set_g, "close", "close gripper"), 'i': (set_g, "open", "open gripper"), '9': (set_g, "calibrate", "calibrate gripper") }) done = False rospy.logwarn("Controlling joints. Press ? for help, Esc to quit.\n\nWARNING: The motion will be slightly jerky!!\n") while not done and not rospy.is_shutdown(): c = getch() if c: #catch Esc or ctrl-c if c in ['\x1b', '\x03']: done = True rospy.signal_shutdown("Example finished.") elif c in bindings: cmd = bindings[c] if c == '8' or c == 'i' or c == '9': cmd[0](cmd[1]) print("command: %s" % (cmd[2],)) else: #expand binding to something like "set_j(right, 'j0', 0.1)" cmd[0](*cmd[1]) print("command: %s" % (cmd[2],)) else: print("key bindings: ") print(" Esc: Quit") print(" ?: Help") for key, val in sorted(viewitems(bindings), key=lambda x: x[1][2]): print(" %s: %s" % (key, val[2])) # rospy.sleep(0.005) def main(): print("Initializing node... ") rospy.init_node("fri_example_joint_position_keyboard") print("Getting robot state... ") def clean_shutdown(): print("\nExiting example.") rospy.on_shutdown(clean_shutdown) map_keyboard() print("Done.") if __name__ == '__main__': main() ================================================ FILE: franka_interface/tests/test_zeroG.py ================================================ import rospy import numpy as np from franka_interface import ArmInterface """ :info: This code is for testing the torque controller. The robot should be in zeroG mode, i.e. It should stay still when no external force is acting, but should be very compliant (almost no resistance) when pushed. This code sends zero torque command continuously, which means the internal gravity compensation is the only torque being sent to the robot. Test by pushing robot. DO NOT RUN THIS CODE WITH CUSTOM END-EFFECTOR UNLESS YOU KNOW WHAT YOU'RE DOING! WARNING: This code only works if the robot model is good! If you have installed custom end-effector, the gravity compensation may not be good unless you have incorporated the model to the FCI via Desk!! """ if __name__ == '__main__': rospy.init_node("test_zeroG") r = ArmInterface() # create arm interface instance (see https://justagist.github.io/franka_ros_interface/DOC.html#arminterface for all available methods for ArmInterface() object) rospy.loginfo("Commanding...\n") r.move_to_neutral() # move robot to neutral pose rate = rospy.Rate(1000) joint_names = r.joint_names() while not rospy.is_shutdown(): r.set_joint_torques(dict(zip(joint_names, [0.0]*7))) # send 0 torques rate.sleep() ================================================ FILE: franka_moveit/CMakeLists.txt ================================================ cmake_minimum_required(VERSION 2.8.3) project(franka_moveit) find_package(catkin REQUIRED COMPONENTS panda_moveit_config ) catkin_package() install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) install(DIRECTORY config DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) catkin_python_setup() catkin_install_python(PROGRAMS scripts/create_demo_planning_scene.py tests/test_scene_interface.py DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) ================================================ FILE: franka_moveit/README.md ================================================ # franka_moveit Some helper classes which interfaces moveit with the Franka Emika Panda robot. ### PandaMoveGroupInterface - Provides interface to control and plan motions using MoveIt in ROS. - Simple methods to plan and execute joint trajectories and cartesian path. - Provides easy reset and environment definition functionalities (See ExtendedPlanningSceneInterface below). ### ExtendedPlanningSceneInterface - Easily define scene for robot motion planning (MoveIt plans will avoid defined obstacles if possible). ================================================ FILE: franka_moveit/config/moveit_demo.rviz ================================================ Panels: - Class: rviz/Displays Help Height: 84 Name: Displays Property Tree Widget: Expanded: - /MotionPlanning1 - /MotionPlanning1/Scene Geometry1 - /MotionPlanning1/Scene Robot1 - /MotionPlanning1/Planning Request1 - /MotionPlanning1/Planned Path1 - /PlanningScene1 - /PlanningScene1/Scene Geometry1 Splitter Ratio: 0.7425600290298462 Tree Height: 126 - Class: rviz/Help Name: Help - Class: rviz/Views Expanded: - /Current View1 Name: Views Splitter Ratio: 0.5 - Class: rviz_visual_tools/RvizVisualToolsGui Name: RvizVisualToolsGui Preferences: PromptSaveOnExit: true Toolbars: toolButtonStyle: 2 Visualization Manager: Class: "" Displays: - Alpha: 0.5 Cell Size: 1 Class: rviz/Grid Color: 160; 160; 164 Enabled: true Line Style: Line Width: 0.029999999329447746 Value: Lines Name: Grid Normal Cell Count: 0 Offset: X: 0 Y: 0 Z: 0 Plane: XY Plane Cell Count: 10 Reference Frame: Value: true - Class: rviz/MarkerArray Enabled: true Marker Topic: /rviz_visual_tools Name: MarkerArray Namespaces: {} Queue Size: 100 Value: true - Acceleration_Scaling_Factor: 0.5 Class: moveit_rviz_plugin/MotionPlanning Enabled: true Move Group Namespace: "" MoveIt_Allow_Approximate_IK: false MoveIt_Allow_External_Program: false MoveIt_Allow_Replanning: false MoveIt_Allow_Sensor_Positioning: false MoveIt_Goal_Tolerance: 0 MoveIt_Planning_Attempts: 10 MoveIt_Planning_Time: 5 MoveIt_Use_Cartesian_Path: false MoveIt_Use_Constraint_Aware_IK: true MoveIt_Warehouse_Host: 127.0.0.1 MoveIt_Warehouse_Port: 33829 MoveIt_Workspace: Center: X: 0 Y: 0 Z: 0 Size: X: 2 Y: 2 Z: 2 Name: MotionPlanning Planned Path: Color Enabled: false Interrupt Display: false Links: All Links Enabled: true Expand Joint Details: false Expand Link Details: false Expand Tree: false Link Tree Style: Links in Alphabetic Order panda_link0: Alpha: 1 Show Axes: false Show Trail: false Value: true panda_link1: Alpha: 1 Show Axes: false Show Trail: false Value: true panda_link2: Alpha: 1 Show Axes: false Show Trail: false Value: true panda_link3: Alpha: 1 Show Axes: false Show Trail: false Value: true panda_link4: Alpha: 1 Show Axes: false Show Trail: false Value: true panda_link5: Alpha: 1 Show Axes: false Show Trail: false Value: true panda_link6: Alpha: 1 Show Axes: false Show Trail: false Value: true panda_link7: Alpha: 1 Show Axes: false Show Trail: false Value: true panda_link8: Alpha: 1 Show Axes: false Show Trail: false world: Alpha: 1 Show Axes: false Show Trail: false Loop Animation: false Robot Alpha: 0.5 Robot Color: 150; 50; 150 Show Robot Collision: false Show Robot Visual: true Show Trail: false State Display Time: 0.05 s Trail Step Size: 1 Trajectory Topic: /move_group/display_planned_path Planning Metrics: Payload: 1 Show Joint Torques: false Show Manipulability: false Show Manipulability Index: false Show Weight Limit: false TextHeight: 0.07999999821186066 Planning Request: Colliding Link Color: 255; 0; 0 Goal State Alpha: 1 Goal State Color: 250; 128; 0 Interactive Marker Size: 0 Joint Violation Color: 255; 0; 255 Planning Group: panda_arm Query Goal State: true Query Start State: false Show Workspace: false Start State Alpha: 1 Start State Color: 0; 255; 0 Planning Scene Topic: /planning_scene Robot Description: robot_description Scene Geometry: Scene Alpha: 0.8999999761581421 Scene Color: 50; 230; 50 Scene Display Time: 0.20000000298023224 Show Scene Geometry: false Voxel Coloring: Z-Axis Voxel Rendering: Occupied Voxels Scene Robot: Attached Body Color: 150; 50; 150 Links: All Links Enabled: true Expand Joint Details: false Expand Link Details: false Expand Tree: false Link Tree Style: Links in Alphabetic Order panda_link0: Alpha: 1 Show Axes: false Show Trail: false Value: true panda_link1: Alpha: 1 Show Axes: false Show Trail: false Value: true panda_link2: Alpha: 1 Show Axes: false Show Trail: false Value: true panda_link3: Alpha: 1 Show Axes: false Show Trail: false Value: true panda_link4: Alpha: 1 Show Axes: false Show Trail: false Value: true panda_link5: Alpha: 1 Show Axes: false Show Trail: false Value: true panda_link6: Alpha: 1 Show Axes: false Show Trail: false Value: true panda_link7: Alpha: 1 Show Axes: false Show Trail: false Value: true panda_link8: Alpha: 1 Show Axes: false Show Trail: false world: Alpha: 1 Show Axes: false Show Trail: false Robot Alpha: 1 Show Robot Collision: false Show Robot Visual: true Value: true Velocity_Scaling_Factor: 0.5 - Class: moveit_rviz_plugin/PlanningScene Enabled: true Move Group Namespace: "" Name: PlanningScene Planning Scene Topic: move_group/monitored_planning_scene Robot Description: robot_description Scene Geometry: Scene Alpha: 1 Scene Color: 170; 255; 255 Scene Display Time: 0.20000000298023224 Show Scene Geometry: true Voxel Coloring: Z-Axis Voxel Rendering: Occupied Voxels Scene Robot: Attached Body Color: 150; 50; 150 Links: All Links Enabled: true Expand Joint Details: false Expand Link Details: false Expand Tree: false Link Tree Style: Links in Alphabetic Order panda_link0: Alpha: 1 Show Axes: false Show Trail: false Value: true panda_link1: Alpha: 1 Show Axes: false Show Trail: false Value: true panda_link2: Alpha: 1 Show Axes: false Show Trail: false Value: true panda_link3: Alpha: 1 Show Axes: false Show Trail: false Value: true panda_link4: Alpha: 1 Show Axes: false Show Trail: false Value: true panda_link5: Alpha: 1 Show Axes: false Show Trail: false Value: true panda_link6: Alpha: 1 Show Axes: false Show Trail: false Value: true panda_link7: Alpha: 1 Show Axes: false Show Trail: false Value: true panda_link8: Alpha: 1 Show Axes: false Show Trail: false world: Alpha: 1 Show Axes: false Show Trail: false Robot Alpha: 1 Show Robot Collision: false Show Robot Visual: true Value: true Enabled: true Global Options: Background Color: 48; 48; 48 Default Light: true Fixed Frame: panda_link0 Frame Rate: 30 Name: root Tools: - Class: rviz/Interact Hide Inactive Objects: true - Class: rviz/MoveCamera - Class: rviz/Select - Class: rviz_visual_tools/KeyTool Value: true Views: Current: Class: rviz/XYOrbit Distance: 2.827594041824341 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: X: 0.11356700211763382 Y: 0.10592000186443329 Z: 2.2351800055275817e-7 Focal Shape Fixed Size: true Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 Pitch: 0.27479690313339233 Target Frame: panda_link0 Value: XYOrbit (rviz) Yaw: 5.819954872131348 Saved: ~ Window Geometry: Displays: collapsed: false Height: 759 Help: collapsed: false Hide Left Dock: false Hide Right Dock: false MotionPlanning: collapsed: false MotionPlanning - Trajectory Slider: collapsed: false QMainWindow State: 000000ff00000000fd000000010000000000000274000002a1fc020000000dfb000000100044006900730070006c006100790073010000003b0000010d000000c700fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a00560069006500770073000000026d000000b5000000a000fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb0000002e004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d00200053006c00690064006500720000000000ffffffff0000000000000000fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e0067010000014e0000018e0000018300fffffffb00000024005200760069007a00560069007300750061006c0054006f006f006c007300470075006900000004bd0000008d0000003f00fffffffb000000120020002d00200053006c00690064006500720000000000ffffffff0000000000000000fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e0067010000034f000001fb0000000000000000fb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e00670100000308000001760000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000003f00ffffff00000326000002a100000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 RvizVisualToolsGui: collapsed: false Views: collapsed: false Width: 1440 X: 297 Y: 101 ================================================ FILE: franka_moveit/launch/demo_moveit.launch ================================================ ================================================ FILE: franka_moveit/package.xml ================================================ franka_moveit 0.7.1 franka_moveit package wrapping franka_moveit_config using franka_ros_interface. Saif Sidhik Apache 2.0 Saif Sidhik catkin panda_moveit_config panda_moveit_config ================================================ FILE: franka_moveit/scripts/create_demo_planning_scene.py ================================================ #!/usr/bin/env python # /*************************************************************************** # # @package: franka_moveit # @metapackage: franka_ros_interface # @author: Saif Sidhik # # **************************************************************************/ # /*************************************************************************** # Copyright (c) 2019-2021, Saif Sidhik # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. # You may obtain a copy of the License at # http://www.apache.org/licenses/LICENSE-2.0 # Unless required by applicable law or agreed to in writing, software # distributed under the License is distributed on an "AS IS" BASIS, # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. # See the License for the specific language governing permissions and # limitations under the License. # **************************************************************************/ import sys import rospy import moveit_commander from franka_moveit import ExtendedPlanningSceneInterface from franka_moveit.utils import create_pose_stamped_msg """ A script for creating a simple environment as a PlanningScene. This script runs by default when interface.launch is started, but can be disabled using argument. """ IRLab_workspace = [ { 'name': 'back_wall', 'pose': create_pose_stamped_msg(position = [-0.57,0.0,0.5], orientation = [1,0,0,0], frame = 'panda_link0'), 'size': [0.1,1.8,1] }, { 'name': 'side_wall', 'pose': create_pose_stamped_msg(position = [-0.3,-0.85,0.5], orientation = [1,0,0,0], frame = 'panda_link0'), 'size': [0.6,0.1,1] }, { 'name': 'table', 'pose': create_pose_stamped_msg(position = [0.45,-0.0,0], orientation = [1,0,0,0], frame = 'panda_link0'), 'size': [2,1.8,0.02] }, { 'name': 'controller_box', 'pose': create_pose_stamped_msg(position = [-0.37,0.55,0.08], orientation = [1,0,0,0], frame = 'panda_link0'), 'size': [0.4,0.6,0.16] }, { 'name': 'equipment_box', 'pose': create_pose_stamped_msg(position = [-0.35,-0.68,0.17], orientation = [1,0,0,0], frame = 'panda_link0'), 'size': [0.46,0.4,0.34] } ] def main(): try: rospy.loginfo("Creating Demo Planning Scene") scene = ExtendedPlanningSceneInterface() rospy.sleep(1) # ----- Not having this delay sometimes caused failing to create some boxes for config in IRLab_workspace: rospy.loginfo("-- Creating object: {}..".format(config['name'])) success = scene.add_box(**config) rospy.loginfo("------ {}".format("success" if success else "FAILED!")) rospy.loginfo("Created Demo Planning Scene.") except rospy.ROSInterruptException: return except KeyboardInterrupt: return if __name__ == '__main__': rospy.init_node('simple_scene_creator', anonymous=True) moveit_commander.roscpp_initialize(sys.argv) main() ================================================ FILE: franka_moveit/setup.py ================================================ #!/usr/bin/env python from distutils.core import setup from catkin_pkg.python_setup import generate_distutils_setup d = generate_distutils_setup() d['packages'] = ['franka_moveit'] d['package_dir'] = {'': 'src'} setup(**d) ================================================ FILE: franka_moveit/src/franka_moveit/__init__.py ================================================ # /*************************************************************************** # Copyright (c) 2019-2021, Saif Sidhik # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. # You may obtain a copy of the License at # http://www.apache.org/licenses/LICENSE-2.0 # Unless required by applicable law or agreed to in writing, software # distributed under the License is distributed on an "AS IS" BASIS, # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. # See the License for the specific language governing permissions and # limitations under the License. # **************************************************************************/ from .extended_planning_scene_interface import ExtendedPlanningSceneInterface from .movegroup_interface import PandaMoveGroupInterface from . import utils ================================================ FILE: franka_moveit/src/franka_moveit/extended_planning_scene_interface.py ================================================ # /*************************************************************************** # # @package: franka_moveit # @metapackage: franka_ros_interface # @author: Saif Sidhik # # **************************************************************************/ # /*************************************************************************** # Copyright (c) 2019-2021, Saif Sidhik # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. # You may obtain a copy of the License at # http://www.apache.org/licenses/LICENSE-2.0 # Unless required by applicable law or agreed to in writing, software # distributed under the License is distributed on an "AS IS" BASIS, # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. # See the License for the specific language governing permissions and # limitations under the License. # **************************************************************************/ import rospy import moveit_commander class ExtendedPlanningSceneInterface(moveit_commander.PlanningSceneInterface): """ .. note:: For other available methods for planning scene interface, refer `PlanningSceneInterface `_. """ def __init__(self): moveit_commander.PlanningSceneInterface.__init__(self) def add_box(self, name, pose, size, timeout = 5): """ Add object to scene and check if it is created. :param name: name of object :param pose: desired pose for the box (Use :py:func:`franka_moveit.utils.create_pose_stamped_msg`) :param size: size of the box :param timeout: time in sec to wait while checking if box is created :type name: str :type pose: geometry_msgs.msg.PoseStamped :type size: [float] (len 3) :type timeout: float """ moveit_commander.PlanningSceneInterface.add_box(self, name = name, pose = pose, size=size) return self._wait_for_state_update(object_name = name, object_is_known=True, timeout=timeout) def _wait_for_state_update(self, object_name, object_is_known=False, object_is_attached=False, timeout=5): start = rospy.get_time() while (rospy.get_time() - start < timeout) and not rospy.is_shutdown(): attached_objects = self.get_attached_objects([object_name]) is_attached = len(list(attached_objects.keys())) > 0 is_known = object_name in self.get_known_object_names() if (object_is_attached == is_attached) and (object_is_known == is_known): return True rospy.sleep(0.1) return False def remove_box(self, box_name, timeout=5): """ Remove box from scene. :param box_name: name of object :param timeout: time in sec to wait while checking if box is created :type box_name: str :type timeout: float """ self.remove_world_object(box_name) return self._wait_for_state_update(object_name = box_name, object_is_attached=False, object_is_known=False, timeout=timeout) if __name__ == '__main__': scene = ExtendedPlanningSceneInterface() ================================================ FILE: franka_moveit/src/franka_moveit/movegroup_interface.py ================================================ # /*************************************************************************** # # @package: franka_moveit # @metapackage: franka_ros_interface # @author: Saif Sidhik # # **************************************************************************/ # /*************************************************************************** # Copyright (c) 2019-2021, Saif Sidhik # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. # You may obtain a copy of the License at # http://www.apache.org/licenses/LICENSE-2.0 # Unless required by applicable law or agreed to in writing, software # distributed under the License is distributed on an "AS IS" BASIS, # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. # See the License for the specific language governing permissions and # limitations under the License. # **************************************************************************/ import sys import copy import rospy import moveit_commander import moveit_msgs.msg import geometry_msgs.msg from moveit_commander.conversions import pose_to_list from franka_moveit import ExtendedPlanningSceneInterface def all_close(goal, actual, tolerance): """ Convenience method for testing if a list of values are within a tolerance of their counterparts in another list :param: goal A list of floats, a Pose or a PoseStamped :param: actual A list of floats, a Pose or a PoseStamped :param: tolerance A float :rtype: bool """ if isinstance(goal, list): for index, g in enumerate(goal): if abs(actual[index] - g) > tolerance: return False elif isinstance(goal, geometry_msgs.msg.PoseStamped): return all_close(goal.pose, actual.pose, tolerance) elif isinstance(goal, geometry_msgs.msg.Pose): return all_close(pose_to_list(goal), pose_to_list(actual), tolerance) return True class PandaMoveGroupInterface: def __init__(self, use_panda_hand_link=False): try: rospy.get_param("/robot_description_semantic") except KeyError: rospy.loginfo(("Moveit server does not seem to be running.")) raise Exception except (socket.error, socket.gaierror): print ("Failed to connect to the ROS parameter server!\n" "Please check to make sure your ROS networking is " "properly configured:\n") sys.exit() moveit_commander.roscpp_initialize(sys.argv) self._robot = moveit_commander.RobotCommander() self._scene = ExtendedPlanningSceneInterface() self._arm_group = moveit_commander.MoveGroupCommander("panda_arm") try: rospy.get_param("/franka_gripper/robot_ip") self._gripper_group = moveit_commander.MoveGroupCommander("hand") except KeyError: rospy.loginfo(("PandaMoveGroupInterface: could not detect gripper.")) self._gripper_group = None except (socket.error, socket.gaierror): print ("Failed to connect to the ROS parameter server!\n" "Please check to make sure your ROS networking is " "properly configured:\n") sys.exit() self._display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory, queue_size=20) if use_panda_hand_link and self._gripper_group is not None: self._default_ee = 'panda_hand' else: self._default_ee = 'panda_link8' self._arm_group.set_end_effector_link(self._default_ee) rospy.logdebug("PandaMoveGroupInterface: Setting default EE link to '{}' " "Use group.set_end_effector_link() method to change default EE.".format(self._default_ee)) self._arm_group.set_max_velocity_scaling_factor(0.3) @property def robot_state_interface(self): """ :getter: The RobotCommander instance of this object :type: moveit_commander.RobotCommander .. note:: For available methods for RobotCommander, refer `RobotCommander `_. """ return self._robot @property def scene(self): """ :getter: The PlanningSceneInterface instance for this robot. This is an interface to the world surrounding the robot :type: franka_moveit.ExtendedPlanningSceneInterface .. note:: For other available methods for planning scene interface, refer `PlanningSceneInterface `_. """ return self._scene @property def arm_group(self): """ :getter: The MoveGroupCommander instance of this object. This is an interface to one group of joints. In this case the group is the joints in the Panda arm. This interface can be used to plan and execute motions on the Panda. :type: moveit_commander.MoveGroupCommander .. note:: For available methods for movegroup, refer `MoveGroupCommander `_. """ return self._arm_group @property def gripper_group(self): """ :getter: The MoveGroupCommander instance of this object. This is an interface to one group of joints. In this case the group is the joints in the Panda gripper. This interface can be used to plan and execute motions of the gripper. :type: moveit_commander.MoveGroupCommander .. note:: For available methods for movegroup, refer `MoveGroupCommander `_. """ return self._gripper_group def go_to_joint_positions(self, positions, wait = True, tolerance = 0.005): """ :return: status of joint motion plan execution :rtype: bool :param positions: target joint positions (ordered) :param wait: if True, function will wait for trajectory execution to complete :param tolerance: maximum error in final position for each joint to consider task a success :type positions: [double] :type wait: bool :type tolerance: double """ self._arm_group.clear_pose_targets() rospy.logdebug("Starting positions: {}".format(self._arm_group.get_current_joint_values())) self._arm_group.go(positions[:7], wait = wait) if len(positions) > 7: self._gripper_group.go(positions[7:], wait = wait) if wait: self._arm_group.stop() gripper_tolerance = True if len(positions)> 7: self._gripper_group.stop() gripper_tolerance = all_close(positions[7:], self._gripper_group.get_current_joint_values(), tolerance) return all_close(positions[:7], self._arm_group.get_current_joint_values(), tolerance) and gripper_tolerance return True def go_to_cartesian_pose(self, pose, ee_link="", wait=True): """ Plan and execute a cartesian path to reach a target by avoiding obstacles in the scene. For planning through multiple points, use func:`self._arm_group.set_pose_targets`. :param pose: The cartesian pose to be reached. (Use :func:`franka_moveit.utils.create_pose_msg` for creating pose messages easily) :type pose: geomentry_msgs.msg.Pose :param ee_link: name of end-effector link to be used; uses currently set value by default :type ee_link: str, optional :param wait: if set to True, blocks till execution is complete; defaults to True :type wait: bool """ self._arm_group.set_pose_target(pose, end_effector_link=ee_link) self._arm_group.go(wait=wait) def plan_cartesian_path(self, poses): """ Plan cartesian path using the provided list of poses. :param poses: The cartesian poses to be achieved in sequence. (Use :func:`franka_moveit.utils.create_pose_msg` for creating pose messages easily) :type poses: [geomentry_msgs.msg.Pose] :return: the actual RobotTrajectory (can be used for :py:meth:`execute_plan`), a fraction of how much of the path was followed :rtype: [RobotTrajectory, float (0,1)] .. 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. """ waypoints = [] for pose in poses: waypoints.append(copy.deepcopy(pose)) plan, fraction = self._arm_group.compute_cartesian_path(waypoints, 0.01, 0.0) return plan, fraction def set_velocity_scale(self, value, group = "arm"): """ Set the max velocity scale for executing planned motion. :param value: scale value (allowed (0,1] ) :type value: float """ if group == "arm": self._arm_group.set_max_velocity_scaling_factor(value) elif group == "gripper": self._gripper_group.set_max_velocity_scaling_factor(value) else: raise ValueError("PandaMoveGroupInterface: Invalid group specified!") rospy.logdebug("PandaMoveGroupInterface: Setting velocity scale to {}".format(value)) def plan_joint_path(self, joint_position): """ :return: RobotTrajectory plan for executing joint trajectory (can be used for :py:meth:`execute_plan`) :param joint_position: target joint positions :type joint_position: [float]*7 """ return self._arm_group.plan(joint_position) def close_gripper(self, wait = False): """ Close gripper. (Using named states defined in urdf.) :param wait: if set to True, blocks till execution is complete; defaults to True :type wait: bool .. note:: If this named state is not found, your ros environment is probably not using the right panda_moveit_config package. Ensure that sourced package is from this repo --> https://github.com/justagist/panda_moveit_config """ self._gripper_group.set_named_target("close") return self._gripper_group.go(wait = wait) def open_gripper(self, wait = False): """ Open gripper. (Using named states defined in urdf) :param wait: if set to True, blocks till execution is complete :type wait: bool .. note:: If this named state is not found, your ros environment is probably not using the right panda_moveit_config package. Ensure that sourced package is from this repo --> https://github.com/justagist/panda_moveit_config. """ self._gripper_group.set_named_target("open") return self._gripper_group.go(wait = wait) def display_trajectory(self, plan): """ Display planned trajectory in RViz. Rviz should be open and Trajectory display should be listening to the appropriate trajectory topic. :param plan: the plan to be executed (from :func:`plan_joint_path` or :py:meth:`plan_cartesian_path`) """ display_trajectory = moveit_msgs.msg.DisplayTrajectory() display_trajectory.trajectory_start = self._robot.get_current_state() display_trajectory.trajectory.append(plan) # Publish self._display_trajectory_publisher.publish(display_trajectory) def move_to_neutral(self, wait = True): """ Send arm group to neutral pose defined using named state in urdf. :param wait: if set to True, blocks till execution is complete :type wait: bool """ self._arm_group.set_named_target("ready") return self._arm_group.go(wait = wait) def execute_plan(self, plan, group = "arm", wait = True): """ Execute the planned trajectory :param plan: The plan to be executed (from :py:meth:`plan_joint_path` or :py:meth:`plan_cartesian_path`) :param group: The name of the move group (default "arm" for robot; use "hand" for gripper group) :type group: str :param wait: if set to True, blocks till execution is complete :type wait: bool """ if group == "arm": self._arm_group.execute(plan, wait = wait) elif group == "gripper": self._gripper_group.execute(plan, wait = wait) else: raise ValueError("PandaMoveGroupInterface: Invalid group specified!") ================================================ FILE: franka_moveit/src/franka_moveit/utils.py ================================================ import geometry_msgs.msg import quaternion def create_pose_msg(position, orientation): """ Create Pose message using the provided position and orientation :return: Pose message for the give end-effector position and orientation :rtype: geometry_msgs.msg.Pose :param position: End-effector position in base frame of the robot :type position: [float]*3 :param orientation: orientation quaternion of end-effector in base frame :type orientation: quaternion.quaternion (OR) [float] size 4: (w,x,y,z) """ pose = geometry_msgs.msg.Pose() pose.position.x = position[0] pose.position.y = position[1] pose.position.z = position[2] if isinstance(orientation,quaternion.quaternion): pose.orientation.x = orientation.x pose.orientation.y = orientation.y pose.orientation.z = orientation.z pose.orientation.w = orientation.w else: pose.orientation.x = orientation[1] pose.orientation.y = orientation[2] pose.orientation.z = orientation[3] pose.orientation.w = orientation[0] return pose def create_pose_stamped_msg(position, orientation, frame = "world"): """ Create PoseStamped message using the provided position and orientation :return: Pose message for the give end-effector position and orientation :rtype: geometry_msgs.msg.Pose :param position: End-effector position in base frame of the robot :type position: [float]*3 :param orientation: orientation quaternion of end-effector in base frame :type orientation: quaternion.quaternion (OR) [float] size 4: (w,x,y,z) :param frame: Name of the parent frame :type frame: str """ pose = geometry_msgs.msg.PoseStamped() pose.header.frame_id = frame pose.pose = create_pose_msg(position, orientation) return pose ================================================ FILE: franka_moveit/tests/test_movegroup_interface.py ================================================ import rospy from franka_moveit import PandaMoveGroupInterface if __name__ == '__main__': rospy.init_node("test_moveit") mvt = PandaMoveGroupInterface() g = mvt.gripper_group r = mvt.arm_group ================================================ FILE: franka_moveit/tests/test_scene_interface.py ================================================ #!/usr/bin/env python import sys import rospy import moveit_commander from builtins import input from franka_moveit import ExtendedPlanningSceneInterface from franka_moveit.utils import create_pose_stamped_msg """ Test if sceneinterface works. Use rviz and add PlanningScene visual while running the interface. """ objects = [ { 'name': 'box1', 'pose': create_pose_stamped_msg(position = [0,0,0], orientation = [1,0,0,0], frame = "world"), 'size': [0.1,0.1,0.1] } , { 'name': 'box2', 'pose': create_pose_stamped_msg(position = [0.5,0,0], orientation = [1,0,0,0], frame = "world"), 'size': [0.2,0.2,0.2] }, { 'name': 'box3', 'pose': create_pose_stamped_msg(position = [0.4,-0.2,0.5], orientation = [1,0,0,0], frame = "world"), 'size': [0.1,0.1,0.1] } ] def main(): print("============ Starting test ...") rospy.sleep(5.) try: input("============ Press `Enter` to begin the tutorial by setting up the scene_interface (press ctrl-d to exit) ...") scene = ExtendedPlanningSceneInterface() input("============ Press `Enter` to add objects to the planning scene ...") for config in objects: scene.add_box(**config) input("============ Press `Enter` to remove the boxes from the planning scene ...") for config in objects: scene.remove_box(config['name']) except rospy.ROSInterruptException: return except KeyboardInterrupt: return if __name__ == '__main__': rospy.init_node('simple_scene_tester', anonymous=True) moveit_commander.roscpp_initialize(sys.argv) main() ================================================ FILE: franka_ros_controllers/CMakeLists.txt ================================================ cmake_minimum_required(VERSION 2.8.3) project(franka_ros_controllers) set(CMAKE_BUILD_TYPE Release) set(CMAKE_CXX_STANDARD 14) set(CMAKE_CXX_STANDARD_REQUIRED ON) find_package(catkin REQUIRED COMPONENTS controller_interface dynamic_reconfigure franka_hw geometry_msgs franka_core_msgs hardware_interface message_generation pluginlib realtime_tools roscpp rospy ) find_package(Eigen3 REQUIRED) find_package(Franka 0.5.0 REQUIRED) generate_dynamic_reconfigure_options( cfg/joint_controller_params.cfg ) catkin_package( LIBRARIES franka_ros_controllers CATKIN_DEPENDS controller_interface dynamic_reconfigure franka_hw geometry_msgs franka_core_msgs hardware_interface message_runtime pluginlib realtime_tools roscpp DEPENDS Franka ) add_library(franka_ros_controllers src/effort_joint_impedance_controller.cpp src/position_joint_position_controller.cpp src/effort_joint_position_controller.cpp src/effort_joint_torque_controller.cpp src/velocity_joint_velocity_controller.cpp ) add_dependencies(franka_ros_controllers ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_gencfg ) target_link_libraries(franka_ros_controllers PUBLIC ${Franka_LIBRARIES} ${catkin_LIBRARIES} ) target_include_directories(franka_ros_controllers SYSTEM PUBLIC ${Franka_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS} ) target_include_directories(franka_ros_controllers PUBLIC include ) ## Installation install(TARGETS franka_ros_controllers ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) install(DIRECTORY config DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} ) install(FILES controller_plugins.xml DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} ) # catkin_install_python( # PROGRAMS scripts/interactive_marker.py # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} # ) ## Tools include(${CMAKE_CURRENT_LIST_DIR}/../cmake/ClangTools.cmake OPTIONAL RESULT_VARIABLE CLANG_TOOLS ) if(CLANG_TOOLS) file(GLOB_RECURSE SOURCES ${CMAKE_CURRENT_SOURCE_DIR}/src/*.cpp) file(GLOB_RECURSE HEADERS ${CMAKE_CURRENT_SOURCE_DIR}/include/*.h ${CMAKE_CURRENT_SOURCE_DIR}/src/*.h ) add_format_target(franka_ros_controllers FILES ${SOURCES} ${HEADERS}) add_tidy_target(franka_ros_controllers FILES ${SOURCES} DEPENDS franka_ros_controllers ) endif() ================================================ FILE: franka_ros_controllers/README.md ================================================ # franka_ros_controllers Controllers for the Franka Emika Panda robot using ROS topics. ### Features: - Includes joint position, joint velocity, and joint effort (direct torque, indirect position and impedance) controllers that can be controlled directly through ROS topic - 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*) - 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. ================================================ FILE: franka_ros_controllers/cfg/joint_controller_params.cfg ================================================ #!/usr/bin/env python PACKAGE = "franka_ros_controllers" from dynamic_reconfigure.parameter_generator_catkin import * gen = ParameterGenerator() """ Joint Position Controller (position_joint_position_controller) smoothing parameter. Increasing the value gives higher smoothing (smaller change in target position per controller update step). """ gen.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) gen.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) """ Joint positions controller gains """ controller_gains = gen.add_group("Controller_Gains") controller_gains.add("j1_k", double_t, 0, "Stiffness parameter of joint 1", 1200, 200, 1500) # (default, min, max) controller_gains.add("j2_k", double_t, 0, "Stiffness parameter of joint 2", 1000, 200, 1500) controller_gains.add("j3_k", double_t, 0, "Stiffness parameter of joint 3", 1000, 200, 1500) controller_gains.add("j4_k", double_t, 0, "Stiffness parameter of joint 4", 800, 200, 1500) controller_gains.add("j5_k", double_t, 0, "Stiffness parameter of joint 5", 300, 100, 1000) controller_gains.add("j6_k", double_t, 0, "Stiffness parameter of joint 6", 200, 75, 500) controller_gains.add("j7_k", double_t, 0, "Stiffness parameter of joint 7", 50, 20, 200) controller_gains.add("j1_d", double_t, 0, "Damping parameter of joint 1", 50, 0, 200) # (default, min, max) controller_gains.add("j2_d", double_t, 0, "Damping parameter of joint 2", 50, 0, 200) controller_gains.add("j3_d", double_t, 0, "Damping parameter of joint 3", 50, 0, 200) controller_gains.add("j4_d", double_t, 0, "Damping parameter of joint 4", 20, 0, 200) controller_gains.add("j5_d", double_t, 0, "Damping parameter of joint 5", 20, 0, 200) controller_gains.add("j6_d", double_t, 0, "Damping parameter of joint 6", 20, 0, 200) controller_gains.add("j7_d", double_t, 0, "Damping parameter of joint 7", 10, 0, 200) exit(gen.generate(PACKAGE, "controller_configurations", "joint_controller_params")) ================================================ FILE: franka_ros_controllers/config/ros_controllers.yaml ================================================ position_joint_position_controller: type: franka_ros_controllers/PositionJointPositionController joint_names: - panda_joint1 - panda_joint2 - panda_joint3 - panda_joint4 - panda_joint5 - panda_joint6 - panda_joint7 velocity_joint_velocity_controller: type: franka_ros_controllers/VelocityJointVelocityController joint_names: - panda_joint1 - panda_joint2 - panda_joint3 - panda_joint4 - panda_joint5 - panda_joint6 - panda_joint7 effort_joint_impedance_controller: type: franka_ros_controllers/EffortJointImpedanceController arm_id: panda joint_names: - panda_joint1 - panda_joint2 - panda_joint3 - panda_joint4 - panda_joint5 - panda_joint6 - panda_joint7 k_gains: # if changing the default values, remember to change in the cfg file as well - 1200.0 - 1000.0 - 1000.0 - 800.0 - 300.0 - 200.0 - 50.0 d_gains: - 50.0 - 50.0 - 50.0 - 20.0 - 20.0 - 20.0 - 10.0 publish_rate: 30.0 coriolis_factor: 1.0 effort_joint_position_controller: type: franka_ros_controllers/EffortJointPositionController arm_id: panda joint_names: - panda_joint1 - panda_joint2 - panda_joint3 - panda_joint4 - panda_joint5 - panda_joint6 - panda_joint7 k_gains: - 1200.0 - 1000.0 - 1000.0 - 800.0 - 300.0 - 200.0 - 50.0 d_gains: - 50.0 - 50.0 - 50.0 - 20.0 - 20.0 - 20.0 - 10.0 publish_rate: 30.0 effort_joint_torque_controller: type: franka_ros_controllers/EffortJointTorqueController arm_id: panda compensate_coriolis: false joint_names: - panda_joint1 - panda_joint2 - panda_joint3 - panda_joint4 - panda_joint5 - panda_joint6 - panda_joint7 ================================================ FILE: franka_ros_controllers/controller_plugins.xml ================================================ Controller for commanding joint position and velocity of the Franka arm using impedance control (torque) Controller for commanding absolute joint positions of the Franka arm Controller for commanding joint position of the Franka arm using effort control (torque) Controller for commanding joint torques of the Franka arm directly Controller for commanding joint velocity of the Franka arm using internal joint velocity controller ================================================ FILE: franka_ros_controllers/include/franka_ros_controllers/effort_joint_impedance_controller.h ================================================ /*************************************************************************** * * @package: franka_ros_controllers * @metapackage: franka_ros_interface * @author: Saif Sidhik * **************************************************************************/ /*************************************************************************** * Copyright (c) 2019-2021, Saif Sidhik. * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. **************************************************************************/ #pragma once #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include namespace franka_ros_controllers { class EffortJointImpedanceController : public controller_interface::MultiInterfaceController< franka_hw::FrankaModelInterface, franka_hw::FrankaStateInterface, hardware_interface::EffortJointInterface> { public: bool init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& node_handle) override; void starting(const ros::Time&) override; void update(const ros::Time&, const ros::Duration& period) override; private: // Saturation std::array saturateTorqueRate( const std::array& tau_d_calculated, const std::array& tau_J_d); // NOLINT (readability-identifier-naming) std::unique_ptr model_handle_; std::vector joint_handles_; static constexpr double kDeltaTauMax{1.0}; double filter_params_{0.005}; std::vector k_gains_; std::vector k_gains_target_; std::vector d_gains_; std::vector d_gains_target_; double coriolis_factor_{1.0}; std::array pos_d_; std::array initial_pos_; std::array prev_pos_; std::array pos_d_target_; std::array dq_d_; std::array dq_filtered_; franka_hw::FrankaStateInterface* franka_state_interface_{}; std::unique_ptr franka_state_handle_{}; franka_hw::TriggerRate rate_trigger_{1.0}; std::array last_tau_d_{}; ros::Subscriber desired_joints_subscriber_; std::unique_ptr< dynamic_reconfigure::Server > dynamic_server_controller_config_; ros::NodeHandle dynamic_reconfigure_controller_gains_node_; franka_core_msgs::JointLimits joint_limits_; franka_hw::TriggerRate trigger_publish_; realtime_tools::RealtimePublisher publisher_controller_states_; bool checkPositionLimits(std::vector positions); bool checkVelocityLimits(std::vector positions); void controllerConfigCallback(franka_ros_controllers::joint_controller_paramsConfig& config, uint32_t level); void jointCmdCallback(const franka_core_msgs::JointCommandConstPtr& msg); }; } // namespace franka_ros_controllers ================================================ FILE: franka_ros_controllers/include/franka_ros_controllers/effort_joint_position_controller.h ================================================ /*************************************************************************** * * @package: franka_ros_controllers * @metapackage: franka_ros_interface * @author: Saif Sidhik * **************************************************************************/ /*************************************************************************** * Copyright (c) 2019-2021, Saif Sidhik. * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. **************************************************************************/ #pragma once #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include namespace franka_ros_controllers { class EffortJointPositionController : public controller_interface::MultiInterfaceController< franka_hw::FrankaStateInterface, hardware_interface::EffortJointInterface> { public: bool init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& node_handle) override; void starting(const ros::Time&) override; void update(const ros::Time&, const ros::Duration& period) override; private: // Saturation std::array saturateTorqueRate( const std::array& tau_d_calculated, const std::array& tau_J_d); // NOLINT (readability-identifier-naming) std::vector joint_handles_; static constexpr double kDeltaTauMax{1.0}; double filter_params_{0.005}; std::vector k_gains_; std::vector k_gains_target_; std::vector d_gains_; std::vector d_gains_target_; double coriolis_factor_{1.0}; std::array pos_d_; std::array initial_pos_; std::array prev_pos_; std::array pos_d_target_; std::array d_error_; std::array p_error_last_; franka_hw::FrankaStateInterface* franka_state_interface_{}; std::unique_ptr franka_state_handle_{}; franka_hw::TriggerRate rate_trigger_{1.0}; ros::Subscriber desired_joints_subscriber_; std::unique_ptr< dynamic_reconfigure::Server > dynamic_server_controller_config_; ros::NodeHandle dynamic_reconfigure_controller_gains_node_; franka_core_msgs::JointLimits joint_limits_; franka_hw::TriggerRate trigger_publish_; realtime_tools::RealtimePublisher publisher_controller_states_; bool checkPositionLimits(std::vector positions); void controllerConfigCallback(franka_ros_controllers::joint_controller_paramsConfig& config, uint32_t level); void jointCmdCallback(const franka_core_msgs::JointCommandConstPtr& msg); }; } // namespace franka_ros_controllers ================================================ FILE: franka_ros_controllers/include/franka_ros_controllers/effort_joint_torque_controller.h ================================================ /*************************************************************************** * * @package: franka_ros_controllers * @metapackage: franka_ros_interface * @author: Saif Sidhik * **************************************************************************/ /*************************************************************************** * Copyright (c) 2019-2021, Saif Sidhik. * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. **************************************************************************/ #pragma once #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include namespace franka_ros_controllers { class EffortJointTorqueController : public controller_interface::MultiInterfaceController< franka_hw::FrankaModelInterface, hardware_interface::EffortJointInterface> { public: bool init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& node_handle) override; void starting(const ros::Time&) override; void update(const ros::Time&, const ros::Duration& period) override; private: // Saturation static std::array saturateTorqueRate( const std::array& tau_d_calculated, const std::array& prev_tau); // NOLINT (readability-identifier-naming) std::unique_ptr model_handle_; std::vector joint_handles_; static constexpr double kDeltaTauMax{1.0}; double coriolis_factor_{1.0}; std::array jnt_cmd_{}; std::array prev_jnt_cmd_{}; franka_hw::TriggerRate rate_trigger_{1.0}; std::array last_tau_d_{}; ros::Subscriber desired_joints_subscriber_; franka_core_msgs::JointLimits joint_limits_; franka_hw::TriggerRate trigger_publish_; realtime_tools::RealtimePublisher publisher_controller_states_; bool checkTorqueLimits(std::vector torques); void jointCmdCallback(const franka_core_msgs::JointCommandConstPtr& msg); }; } // namespace franka_ros_controllers ================================================ FILE: franka_ros_controllers/include/franka_ros_controllers/position_joint_position_controller.h ================================================ /*************************************************************************** * * @package: franka_ros_controllers * @metapackage: franka_ros_interface * @author: Saif Sidhik * **************************************************************************/ /*************************************************************************** * Copyright (c) 2019-2021, Saif Sidhik. * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. **************************************************************************/ #pragma once #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include namespace franka_ros_controllers { class PositionJointPositionController : public controller_interface::MultiInterfaceController< hardware_interface::PositionJointInterface> { public: bool init(hardware_interface::RobotHW* robot_hardware, ros::NodeHandle& node_handle) override; void starting(const ros::Time&) override; void update(const ros::Time&, const ros::Duration& period) override; private: hardware_interface::PositionJointInterface* position_joint_interface_; std::vector position_joint_handles_; std::array initial_pos_{}; std::array prev_pos_{}; std::array pos_d_target_{}; std::array pos_d_; // joint_cmd subscriber ros::Subscriber desired_joints_subscriber_; double filter_joint_pos_{0.3}; double target_filter_joint_pos_{0.3}; double filter_factor_{0.01}; double param_change_filter_{0.005}; franka_core_msgs::JointLimits joint_limits_; // Dynamic reconfigure std::unique_ptr< dynamic_reconfigure::Server > dynamic_server_joint_controller_params_; ros::NodeHandle dynamic_reconfigure_joint_controller_params_node_; franka_hw::TriggerRate trigger_publish_; realtime_tools::RealtimePublisher publisher_controller_states_; bool checkPositionLimits(std::vector positions); void jointControllerParamCallback(franka_ros_controllers::joint_controller_paramsConfig& config, uint32_t level); void jointPosCmdCallback(const franka_core_msgs::JointCommandConstPtr& msg); }; } // namespace franka_ros_controllers ================================================ FILE: franka_ros_controllers/include/franka_ros_controllers/velocity_joint_velocity_controller.h ================================================ /*************************************************************************** * * @package: franka_ros_controllers * @metapackage: franka_ros_interface * @author: Saif Sidhik * **************************************************************************/ /*************************************************************************** * Copyright (c) 2019-2021, Saif Sidhik. * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. **************************************************************************/ #pragma once #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include namespace franka_ros_controllers { class VelocityJointVelocityController : public controller_interface::MultiInterfaceController< hardware_interface::VelocityJointInterface, franka_hw::FrankaStateInterface> { public: bool init(hardware_interface::RobotHW* robot_hardware, ros::NodeHandle& node_handle) override; void update(const ros::Time&, const ros::Duration& period) override; void starting(const ros::Time&) override; void stopping(const ros::Time&) override; private: hardware_interface::VelocityJointInterface* velocity_joint_interface_; std::vector velocity_joint_handles_; std::array initial_vel_{}; std::array prev_d_{}; std::array vel_d_target_{}; std::array vel_d_; // joint_cmd subscriber ros::Subscriber desired_joints_subscriber_; double filter_joint_vel_{0.3}; double target_filter_joint_vel_{0.3}; double filter_factor_{0.01}; double param_change_filter_{0.005}; franka_core_msgs::JointLimits joint_limits_; // Dynamic reconfigure std::unique_ptr< dynamic_reconfigure::Server > dynamic_server_joint_controller_params_; ros::NodeHandle dynamic_reconfigure_joint_controller_params_node_; franka_hw::TriggerRate trigger_publish_; realtime_tools::RealtimePublisher publisher_controller_states_; bool checkVelocityLimits(std::vector velocities); void jointControllerParamCallback(franka_ros_controllers::joint_controller_paramsConfig& config, uint32_t level); void jointVelCmdCallback(const franka_core_msgs::JointCommandConstPtr& msg); }; } // namespace franka_ros_controllers ================================================ FILE: franka_ros_controllers/package.xml ================================================ franka_ros_controllers 0.7.1 test control Saif Sidhik Apache 2.0 Saif Sidhik catkin message_generation eigen message_runtime controller_interface dynamic_reconfigure franka_hw geometry_msgs hardware_interface franka_core_msgs libfranka pluginlib realtime_tools roscpp franka_control franka_description message_runtime rospy ================================================ FILE: franka_ros_controllers/scripts/test_controller.py ================================================ import rospy from franka_interface import ArmInterface import numpy as np from builtins import input # import matplotlib.pyplot as plt # from std_msgs.msg import Float64 from copy import deepcopy names = ['panda_joint1','panda_joint2','panda_joint3','panda_joint4','panda_joint5','panda_joint6','panda_joint7'] if __name__ == '__main__': rospy.init_node("test_node") r = ArmInterface() rate = rospy.Rate(400) elapsed_time_ = rospy.Duration(0.0) period = rospy.Duration(0.005) r.move_to_neutral() # move to neutral pose before beginning initial_pose = deepcopy(r.joint_ordered_angles()) input("Hit Enter to Start") print("commanding") vals = deepcopy(initial_pose) count = 0 while not rospy.is_shutdown(): elapsed_time_ += period delta = 3.14 / 16.0 * (1 - np.cos(3.14 / 5.0 * elapsed_time_.to_sec())) * 0.2 for j, _ in enumerate(vals): if j == 4: vals[j] = initial_pose[j] - delta else: vals[j] = initial_pose[j] + delta if count%500 == 0: print(vals, delta) print("\n ---- \n") print(" ") # r.set_joint_positions_velocities(vals, [0.0 for _ in range(7)]) # for impedance control r.set_joint_positions(dict(zip(r.joint_names(), vals))) # try this for position control count += 1 rate.sleep() ================================================ FILE: franka_ros_controllers/src/effort_joint_impedance_controller.cpp ================================================ /*************************************************************************** * * @package: franka_ros_controllers * @metapackage: franka_ros_interface * @author: Saif Sidhik * **************************************************************************/ /*************************************************************************** * Copyright (c) 2019-2021, Saif Sidhik. * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. **************************************************************************/ #include #include #include #include #include #include #include namespace franka_ros_controllers { bool EffortJointImpedanceController::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& node_handle) { std::string arm_id; if (!node_handle.getParam("/robot_config/arm_id", arm_id)) { ROS_ERROR("EffortJointImpedanceController: Could not read parameter arm_id"); return false; } franka_state_interface_ = robot_hw->get(); if (franka_state_interface_ == nullptr) { ROS_ERROR("EffortJointImpedanceController: Could not get Franka state interface from hardware"); return false; } if (!node_handle.getParam("/robot_config/joint_names", joint_limits_.joint_names) || joint_limits_.joint_names.size() != 7) { ROS_ERROR( "EffortJointImpedanceController: Invalid or no joint_names parameters provided, aborting " "controller init!"); return false; } if (!node_handle.getParam("k_gains", k_gains_) || k_gains_.size() != 7) { ROS_ERROR( "EffortJointImpedanceController: Invalid or no k_gain parameters provided, aborting " "controller init!"); return false; } if (!node_handle.getParam("d_gains", d_gains_) || d_gains_.size() != 7) { ROS_ERROR( "EffortJointImpedanceController: Invalid or no d_gain parameters provided, aborting " "controller init!"); return false; } k_gains_target_.clear(); d_gains_target_.clear(); // k_gains_target_.resize(k_gains_.size()); // d_gains_target_.resize(d_gains_.size()); std::map pos_limit_lower_map; std::map pos_limit_upper_map; if (!node_handle.getParam("/robot_config/joint_config/joint_position_limit/lower", pos_limit_lower_map) ) { ROS_ERROR( "EffortJointImpedanceController: Joint limits parameters not provided, aborting " "controller init!"); return false; } if (!node_handle.getParam("/robot_config/joint_config/joint_position_limit/upper", pos_limit_upper_map) ) { ROS_ERROR( "EffortJointImpedanceController: Joint limits parameters not provided, aborting " "controller init!"); return false; } std::map velocity_limit_map; if (!node_handle.getParam("/robot_config/joint_config/joint_velocity_limit", velocity_limit_map)) { ROS_ERROR("EffortJointImpedanceController: Failed to find joint velocity limits on the param server. Aborting controller init"); return false; } for (size_t i = 0; i < joint_limits_.joint_names.size(); ++i){ if (pos_limit_lower_map.find(joint_limits_.joint_names[i]) != pos_limit_lower_map.end()) { joint_limits_.position_lower.push_back(pos_limit_lower_map[joint_limits_.joint_names[i]]); } else { ROS_ERROR("EffortJointImpedanceController: Unable to find lower position limit values for joint %s...", joint_limits_.joint_names[i].c_str()); } if (pos_limit_upper_map.find(joint_limits_.joint_names[i]) != pos_limit_upper_map.end()) { joint_limits_.position_upper.push_back(pos_limit_upper_map[joint_limits_.joint_names[i]]); } else { ROS_ERROR("EffortJointImpedanceController: Unable to find upper position limit values for joint %s...", joint_limits_.joint_names[i].c_str()); } if (velocity_limit_map.find(joint_limits_.joint_names[i]) != velocity_limit_map.end()) { joint_limits_.velocity.push_back(velocity_limit_map[joint_limits_.joint_names[i]]); } else { ROS_ERROR("EffortJointImpedanceController: Unable to find velocity limit values for joint %s...", joint_limits_.joint_names[i].c_str()); } // ROS_INFO_STREAM("K: "<get(); if (model_interface == nullptr) { ROS_ERROR_STREAM( "EffortJointImpedanceController: Error getting model interface from hardware"); return false; } try { model_handle_ = std::make_unique( model_interface->getHandle(arm_id + "_model")); } catch (hardware_interface::HardwareInterfaceException& ex) { ROS_ERROR_STREAM( "EffortJointImpedanceController: Exception getting model handle from interface: " << ex.what()); return false; } try { franka_state_handle_ = std::make_unique( franka_state_interface_->getHandle(arm_id + "_robot")); } catch (const hardware_interface::HardwareInterfaceException& ex) { ROS_ERROR_STREAM("EffortJointImpedanceController: Exception getting franka state handle: " << ex.what()); return false; } auto* effort_joint_interface = robot_hw->get(); if (effort_joint_interface == nullptr) { ROS_ERROR_STREAM( "EffortJointImpedanceController: Error getting effort joint interface from hardware"); return false; } for (size_t i = 0; i < 7; ++i) { try { joint_handles_.push_back(effort_joint_interface->getHandle(joint_limits_.joint_names[i])); } catch (const hardware_interface::HardwareInterfaceException& ex) { ROS_ERROR_STREAM( "EffortJointImpedanceController: Exception getting joint handles: " << ex.what()); return false; } k_gains_target_.push_back(k_gains_[i]); d_gains_target_.push_back(d_gains_[i]); } dynamic_reconfigure_controller_gains_node_ = ros::NodeHandle("/franka_ros_interface/effort_joint_impedance_controller/arm/controller_parameters_config"); dynamic_server_controller_config_ = std::make_unique< dynamic_reconfigure::Server>( dynamic_reconfigure_controller_gains_node_); dynamic_server_controller_config_->setCallback( boost::bind(&EffortJointImpedanceController::controllerConfigCallback, this, _1, _2)); desired_joints_subscriber_ = node_handle.subscribe( "/franka_ros_interface/motion_controller/arm/joint_commands", 20, &EffortJointImpedanceController::jointCmdCallback, this, ros::TransportHints().reliable().tcpNoDelay()); publisher_controller_states_.init(node_handle, "/franka_ros_interface/motion_controller/arm/joint_controller_states", 1); { std::lock_guard > lock( publisher_controller_states_); publisher_controller_states_.msg_.controller_name = "effort_joint_impedance_controller"; publisher_controller_states_.msg_.names.resize(joint_limits_.joint_names.size()); publisher_controller_states_.msg_.joint_controller_states.resize(joint_limits_.joint_names.size()); } std::fill(dq_filtered_.begin(), dq_filtered_.end(), 0); 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!? k_gains_target_[i] = k_gains_[i]; d_gains_target_[i] = d_gains_[i]; } return true; } void EffortJointImpedanceController::starting(const ros::Time& /*time*/) { franka::RobotState robot_state = franka_state_handle_->getRobotState(); for (size_t i = 0; i < 7; ++i) { initial_pos_[i] = robot_state.q[i]; std::cout << "---- Joint Number: " << i << std::endl; std::cout << "Joint Pos: " << initial_pos_[i] << std::endl; std::cout << "K_des: " << k_gains_target_[i] << std::endl; std::cout << "D_des: " << d_gains_target_[i] << std::endl; std::cout << std::endl; } prev_pos_ = initial_pos_; pos_d_target_ = initial_pos_; std::fill(dq_filtered_.begin(), dq_filtered_.end(), 0); dq_d_ = dq_filtered_; } void EffortJointImpedanceController::update(const ros::Time& time, const ros::Duration& period) { franka::RobotState robot_state = franka_state_handle_->getRobotState(); std::array coriolis = model_handle_->getCoriolis(); double alpha = 0.99; for (size_t i = 0; i < 7; i++) { dq_filtered_[i] = (1 - alpha) * dq_filtered_[i] + alpha * robot_state.dq[i]; } std::array tau_d_calculated{}; for (size_t i = 0; i < 7; ++i) { tau_d_calculated[i] = coriolis_factor_ * coriolis[i] + k_gains_[i] * (pos_d_target_[i] - robot_state.q[i]) + d_gains_[i] * (dq_d_[i] - dq_filtered_[i]); } // Maximum torque difference with a sampling rate of 1 kHz. The maximum torque rate is // 1000 * (1 / sampling_time). std::array tau_d_saturated = saturateTorqueRate(tau_d_calculated, robot_state.tau_J_d); if (trigger_publish_() && publisher_controller_states_.trylock()) { for (size_t i = 0; i < 7; ++i){ publisher_controller_states_.msg_.joint_controller_states[i].set_point = pos_d_target_[i]; publisher_controller_states_.msg_.joint_controller_states[i].process_value = robot_state.q[i]; publisher_controller_states_.msg_.joint_controller_states[i].process_value_dot = robot_state.dq[i]; publisher_controller_states_.msg_.joint_controller_states[i].error = pos_d_target_[i] - robot_state.q[i]; publisher_controller_states_.msg_.joint_controller_states[i].time_step = period.toSec(); publisher_controller_states_.msg_.joint_controller_states[i].command = tau_d_calculated[i]; publisher_controller_states_.msg_.joint_controller_states[i].p = k_gains_[i]; publisher_controller_states_.msg_.joint_controller_states[i].d = d_gains_[i]; publisher_controller_states_.msg_.joint_controller_states[i].header.stamp = time; } publisher_controller_states_.unlockAndPublish(); } for (size_t i = 0; i < 7; ++i) { joint_handles_[i].setCommand(tau_d_saturated[i]); prev_pos_[i] = robot_state.q[i]; k_gains_[i] = filter_params_ * k_gains_target_[i] + (1.0 - filter_params_) * k_gains_[i]; d_gains_[i] = filter_params_ * d_gains_target_[i] + (1.0 - filter_params_) * d_gains_[i]; } } bool EffortJointImpedanceController::checkPositionLimits(std::vector positions) { for (size_t i = 0; i < 7; ++i){ if (!((positions[i] <= joint_limits_.position_upper[i]) && (positions[i] >= joint_limits_.position_lower[i]))){ return true; } } return false; } bool EffortJointImpedanceController::checkVelocityLimits(std::vector velocities) { // bool retval = true; for (size_t i = 0; i < 7; ++i){ if (!(abs(velocities[i]) <= joint_limits_.velocity[i])){ return true; } } return false; } std::array EffortJointImpedanceController::saturateTorqueRate( const std::array& tau_d_calculated, const std::array& tau_J_d) { // NOLINT (readability-identifier-naming) std::array tau_d_saturated{}; for (size_t i = 0; i < 7; i++) { double difference = tau_d_calculated[i] - tau_J_d[i]; tau_d_saturated[i] = tau_J_d[i] + std::max(std::min(difference, kDeltaTauMax), -kDeltaTauMax); } return tau_d_saturated; } void EffortJointImpedanceController::jointCmdCallback(const franka_core_msgs::JointCommandConstPtr& msg) { if (msg->mode == franka_core_msgs::JointCommand::IMPEDANCE_MODE){ if (msg->position.size() != 7) { ROS_ERROR_STREAM( "EffortJointImpedanceController: Published Commands are not of size 7"); pos_d_target_ = prev_pos_; } else if (checkPositionLimits(msg->position) || checkVelocityLimits(msg->velocity)) { ROS_ERROR_STREAM( "EffortJointImpedanceController: Commanded positions or velicities are beyond allowed position limits."); pos_d_target_ = prev_pos_; } else { std::copy_n(msg->position.begin(), 7, pos_d_target_.begin()); std::copy_n(msg->velocity.begin(), 7, dq_d_.begin()); // if velocity is not there, the controller fails!! } } // else ROS_ERROR_STREAM("EffortJointImpedanceController: Published Command msg are not of JointCommand::IMPEDANCE_MODE! Dropping message"); } void EffortJointImpedanceController::controllerConfigCallback( franka_ros_controllers::joint_controller_paramsConfig& config, uint32_t /*level*/) { ROS_DEBUG_STREAM("EffortJointImpedanceController: Updating Config"); k_gains_target_[0] = config.groups.controller_gains.j1_k; k_gains_target_[1] = config.groups.controller_gains.j2_k; k_gains_target_[2] = config.groups.controller_gains.j3_k; k_gains_target_[3] = config.groups.controller_gains.j4_k; k_gains_target_[4] = config.groups.controller_gains.j5_k; k_gains_target_[5] = config.groups.controller_gains.j6_k; k_gains_target_[6] = config.groups.controller_gains.j7_k; d_gains_target_[0] = config.groups.controller_gains.j1_d; d_gains_target_[1] = config.groups.controller_gains.j2_d; d_gains_target_[2] = config.groups.controller_gains.j3_d; d_gains_target_[3] = config.groups.controller_gains.j4_d; d_gains_target_[4] = config.groups.controller_gains.j5_d; d_gains_target_[5] = config.groups.controller_gains.j6_d; d_gains_target_[6] = config.groups.controller_gains.j7_d; } } // namespace franka_ros_controllers PLUGINLIB_EXPORT_CLASS(franka_ros_controllers::EffortJointImpedanceController, controller_interface::ControllerBase) ================================================ FILE: franka_ros_controllers/src/effort_joint_position_controller.cpp ================================================ /*************************************************************************** * * @package: franka_ros_controllers * @metapackage: franka_ros_interface * @author: Saif Sidhik * **************************************************************************/ /*************************************************************************** * Copyright (c) 2019-2021, Saif Sidhik. * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. **************************************************************************/ #include #include #include #include #include #include #include namespace franka_ros_controllers { bool EffortJointPositionController::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& node_handle) { std::string arm_id; if (!node_handle.getParam("/robot_config/arm_id", arm_id)) { ROS_ERROR("EffortJointPositionController: Could not read parameter arm_id"); return false; } franka_state_interface_ = robot_hw->get(); if (franka_state_interface_ == nullptr) { ROS_ERROR("EffortJointPositionController: Could not get Franka state interface from hardware"); return false; } if (!node_handle.getParam("/robot_config/joint_names", joint_limits_.joint_names) || joint_limits_.joint_names.size() != 7) { ROS_ERROR( "EffortJointPositionController: Invalid or no joint_names parameters provided, aborting " "controller init!"); return false; } if (!node_handle.getParam("k_gains", k_gains_) || k_gains_.size() != 7) { ROS_ERROR( "EffortJointPositionController: Invalid or no k_gain parameters provided, aborting " "controller init!"); return false; } if (!node_handle.getParam("d_gains", d_gains_) || d_gains_.size() != 7) { ROS_ERROR( "EffortJointPositionController: Invalid or no d_gain parameters provided, aborting " "controller init!"); return false; } k_gains_target_.clear(); d_gains_target_.clear(); std::map pos_limit_lower_map; std::map pos_limit_upper_map; if (!node_handle.getParam("/robot_config/joint_config/joint_position_limit/lower", pos_limit_lower_map) ) { ROS_ERROR( "EffortJointPositionController: Joint limits parameters not provided, aborting " "controller init!"); return false; } if (!node_handle.getParam("/robot_config/joint_config/joint_position_limit/upper", pos_limit_upper_map) ) { ROS_ERROR( "EffortJointPositionController: Joint limits parameters not provided, aborting " "controller init!"); return false; } for (size_t i = 0; i < joint_limits_.joint_names.size(); ++i){ if (pos_limit_lower_map.find(joint_limits_.joint_names[i]) != pos_limit_lower_map.end()) { joint_limits_.position_lower.push_back(pos_limit_lower_map[joint_limits_.joint_names[i]]); } else { ROS_ERROR("EffortJointPositionController: Unable to find lower position limit values for joint %s...", joint_limits_.joint_names[i].c_str()); } if (pos_limit_upper_map.find(joint_limits_.joint_names[i]) != pos_limit_upper_map.end()) { joint_limits_.position_upper.push_back(pos_limit_upper_map[joint_limits_.joint_names[i]]); } else { ROS_ERROR("EffortJointPositionController: Unable to find upper position limit values for joint %s...", joint_limits_.joint_names[i].c_str()); } } double controller_state_publish_rate(30.0); if (!node_handle.getParam("controller_state_publish_rate", controller_state_publish_rate)) { ROS_INFO_STREAM("EffortJointPositionController: Did not find controller_state_publish_rate. Using default " << controller_state_publish_rate << " [Hz]."); } trigger_publish_ = franka_hw::TriggerRate(controller_state_publish_rate); try { franka_state_handle_ = std::make_unique( franka_state_interface_->getHandle(arm_id + "_robot")); } catch (const hardware_interface::HardwareInterfaceException& ex) { ROS_ERROR_STREAM("EffortJointPositionController: Exception getting franka state handle: " << ex.what()); return false; } auto* effort_joint_interface = robot_hw->get(); if (effort_joint_interface == nullptr) { ROS_ERROR_STREAM( "EffortJointPositionController: Error getting effort joint interface from hardware"); return false; } for (size_t i = 0; i < 7; ++i) { try { joint_handles_.push_back(effort_joint_interface->getHandle(joint_limits_.joint_names[i])); } catch (const hardware_interface::HardwareInterfaceException& ex) { ROS_ERROR_STREAM( "EffortJointPositionController: Exception getting joint handles: " << ex.what()); return false; } k_gains_target_.push_back(k_gains_[i]); d_gains_target_.push_back(d_gains_[i]); } dynamic_reconfigure_controller_gains_node_ = ros::NodeHandle("/franka_ros_interface/effort_joint_position_controller/arm/controller_parameters_config"); dynamic_server_controller_config_ = std::make_unique< dynamic_reconfigure::Server>( dynamic_reconfigure_controller_gains_node_); dynamic_server_controller_config_->setCallback( boost::bind(&EffortJointPositionController::controllerConfigCallback, this, _1, _2)); desired_joints_subscriber_ = node_handle.subscribe( "/franka_ros_interface/motion_controller/arm/joint_commands", 20, &EffortJointPositionController::jointCmdCallback, this, ros::TransportHints().reliable().tcpNoDelay()); publisher_controller_states_.init(node_handle, "/franka_ros_interface/motion_controller/arm/joint_controller_states", 1); { std::lock_guard > lock( publisher_controller_states_); publisher_controller_states_.msg_.controller_name = "effort_joint_position_controller"; publisher_controller_states_.msg_.names.resize(joint_limits_.joint_names.size()); publisher_controller_states_.msg_.joint_controller_states.resize(joint_limits_.joint_names.size()); } 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!? k_gains_target_[i] = k_gains_[i]; d_gains_target_[i] = d_gains_[i]; } return true; } void EffortJointPositionController::starting(const ros::Time& /*time*/) { franka::RobotState robot_state = franka_state_handle_->getRobotState(); for (size_t i = 0; i < 7; ++i) { initial_pos_[i] = robot_state.q[i]; } prev_pos_ = initial_pos_; pos_d_target_ = initial_pos_; std::fill(p_error_last_.begin(), p_error_last_.end(), 0); d_error_ = p_error_last_; } void EffortJointPositionController::update(const ros::Time& time, const ros::Duration& period) { franka::RobotState robot_state = franka_state_handle_->getRobotState(); std::array error = p_error_last_; std::array error_dot = d_error_; for (size_t i = 0; i < 7; i++) { error[i] = (pos_d_target_[i] - robot_state.q[i]); if (period.toSec() > 0.0) { error_dot[i] = (error[i] - p_error_last_[i]) / period.toSec(); } } d_error_ = error_dot; // Compute torque command using PD control law std::array tau_d_calculated{}; for (size_t i = 0; i < 7; ++i) { tau_d_calculated[i] = k_gains_[i] * error[i] + d_gains_[i] * d_error_[i]; } p_error_last_ = error; // Maximum torque difference with a sampling rate of 1 kHz. The maximum torque rate is // 1000 * (1 / sampling_time). std::array tau_d_saturated = saturateTorqueRate(tau_d_calculated, robot_state.tau_J_d); if (trigger_publish_() && publisher_controller_states_.trylock()) { for (size_t i = 0; i < 7; ++i){ publisher_controller_states_.msg_.joint_controller_states[i].set_point = pos_d_target_[i]; publisher_controller_states_.msg_.joint_controller_states[i].process_value = robot_state.q[i]; publisher_controller_states_.msg_.joint_controller_states[i].process_value_dot = d_error_[i]; publisher_controller_states_.msg_.joint_controller_states[i].error = error[i]; publisher_controller_states_.msg_.joint_controller_states[i].time_step = period.toSec(); publisher_controller_states_.msg_.joint_controller_states[i].command = tau_d_calculated[i]; publisher_controller_states_.msg_.joint_controller_states[i].p = k_gains_[i]; publisher_controller_states_.msg_.joint_controller_states[i].d = d_gains_[i]; publisher_controller_states_.msg_.joint_controller_states[i].header.stamp = time; } publisher_controller_states_.unlockAndPublish(); } for (size_t i = 0; i < 7; ++i) { joint_handles_[i].setCommand(tau_d_saturated[i]); prev_pos_[i] = robot_state.q[i]; k_gains_[i] = filter_params_ * k_gains_target_[i] + (1.0 - filter_params_) * k_gains_[i]; d_gains_[i] = filter_params_ * d_gains_target_[i] + (1.0 - filter_params_) * d_gains_[i]; } } bool EffortJointPositionController::checkPositionLimits(std::vector positions) { for (size_t i = 0; i < 7; ++i){ if (!((positions[i] <= joint_limits_.position_upper[i]) && (positions[i] >= joint_limits_.position_lower[i]))){ return true; } } return false; } std::array EffortJointPositionController::saturateTorqueRate( const std::array& tau_d_calculated, const std::array& tau_J_d) { // NOLINT (readability-identifier-naming) std::array tau_d_saturated{}; for (size_t i = 0; i < 7; i++) { double difference = tau_d_calculated[i] - tau_J_d[i]; tau_d_saturated[i] = tau_J_d[i] + std::max(std::min(difference, kDeltaTauMax), -kDeltaTauMax); } return tau_d_saturated; } void EffortJointPositionController::jointCmdCallback(const franka_core_msgs::JointCommandConstPtr& msg) { if (msg->mode == franka_core_msgs::JointCommand::POSITION_MODE){ if (msg->position.size() != 7) { ROS_ERROR_STREAM( "EffortJointPositionController: Published Commands are not of size 7"); pos_d_target_ = prev_pos_; std::fill(p_error_last_.begin(), p_error_last_.end(), 0); d_error_ = p_error_last_; } else if (checkPositionLimits(msg->position)) { ROS_ERROR_STREAM( "PositionJointPositionController: Commanded positions are beyond allowed position limits."); pos_d_target_ = prev_pos_; std::fill(p_error_last_.begin(), p_error_last_.end(), 0); d_error_ = p_error_last_; } else { std::copy_n(msg->position.begin(), 7, pos_d_target_.begin()); } } // else ROS_ERROR_STREAM("EffortJointPositionController: Published Command msg are not of JointCommand::POSITION_MODE! Dropping message"); } void EffortJointPositionController::controllerConfigCallback( franka_ros_controllers::joint_controller_paramsConfig& config, uint32_t /*level*/) { k_gains_target_[0] = config.j1_k; k_gains_target_[1] = config.j2_k; k_gains_target_[2] = config.j3_k; k_gains_target_[3] = config.j4_k; k_gains_target_[4] = config.j5_k; k_gains_target_[5] = config.j6_k; k_gains_target_[6] = config.j7_k; d_gains_target_[0] = config.j1_d; d_gains_target_[1] = config.j2_d; d_gains_target_[2] = config.j3_d; d_gains_target_[3] = config.j4_d; d_gains_target_[4] = config.j5_d; d_gains_target_[5] = config.j6_d; d_gains_target_[6] = config.j7_d; } } // namespace franka_ros_controllers PLUGINLIB_EXPORT_CLASS(franka_ros_controllers::EffortJointPositionController, controller_interface::ControllerBase) ================================================ FILE: franka_ros_controllers/src/effort_joint_torque_controller.cpp ================================================ /*************************************************************************** * * @package: franka_ros_controllers * @metapackage: franka_ros_interface * @author: Saif Sidhik * **************************************************************************/ /*************************************************************************** * Copyright (c) 2019-2021, Saif Sidhik. * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. **************************************************************************/ #include #include #include #include #include #include namespace franka_ros_controllers { bool EffortJointTorqueController::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& node_handle) { std::string arm_id; if (!node_handle.getParam("/robot_config/arm_id", arm_id)) { ROS_ERROR("EffortJointTorqueController: Could not read parameter arm_id"); return false; } if (!node_handle.getParam("/robot_config/joint_names", joint_limits_.joint_names) || joint_limits_.joint_names.size() != 7) { ROS_ERROR( "EffortJointTorqueController: Invalid or no joint_names parameters provided, aborting " "controller init!"); return false; } bool enable_coriolis; if (!node_handle.getParam("/franka_ros_interface/effort_joint_torque_controller/compensate_coriolis", enable_coriolis)) { ROS_ERROR("EffortJointTorqueController: Could not read parameter compensate_coriolis"); return false; } if (enable_coriolis==false){ coriolis_factor_ = 0.0; ROS_INFO_STREAM("EffortJointTorqueController: Coriolis compensation disabled!"); } else{ ROS_INFO_STREAM("EffortJointTorqueController: Coriolis compensation enabled!"); } std::map torque_limit_map; if (!node_handle.getParam("/robot_config/joint_config/joint_effort_limit", torque_limit_map)) { ROS_ERROR("EffortJointTorqueController: Failed to find joint effort limits on the param server. Aborting controller init"); return false; } for (size_t i = 0; i < joint_limits_.joint_names.size(); ++i){ if (torque_limit_map.find(joint_limits_.joint_names[i]) != torque_limit_map.end()) { joint_limits_.effort.push_back(torque_limit_map[joint_limits_.joint_names[i]]); } else { ROS_ERROR("EffortJointTorqueController: Unable to find torque limit values for joint %s...", joint_limits_.joint_names[i].c_str()); } } double controller_state_publish_rate(30.0); if (!node_handle.getParam("controller_state_publish_rate", controller_state_publish_rate)) { ROS_INFO_STREAM("EffortJointTorqueController: Did not find controller_state_publish_rate. Using default " << controller_state_publish_rate << " [Hz]."); } trigger_publish_ = franka_hw::TriggerRate(controller_state_publish_rate); auto* model_interface = robot_hw->get(); if (model_interface == nullptr) { ROS_ERROR_STREAM( "EffortJointTorqueController: Error getting model interface from hardware"); return false; } try { model_handle_ = std::make_unique( model_interface->getHandle(arm_id + "_model")); } catch (hardware_interface::HardwareInterfaceException& ex) { ROS_ERROR_STREAM( "EffortJointTorqueController: Exception getting model handle from interface: " << ex.what()); return false; } auto* effort_joint_interface = robot_hw->get(); if (effort_joint_interface == nullptr) { ROS_ERROR_STREAM( "EffortJointTorqueController: Error getting effort joint interface from hardware"); return false; } for (size_t i = 0; i < 7; ++i) { try { joint_handles_.push_back(effort_joint_interface->getHandle(joint_limits_.joint_names[i])); } catch (const hardware_interface::HardwareInterfaceException& ex) { ROS_ERROR_STREAM( "EffortJointTorqueController: Exception getting joint handles: " << ex.what()); return false; } } desired_joints_subscriber_ = node_handle.subscribe( "/franka_ros_interface/motion_controller/arm/joint_commands", 20, &EffortJointTorqueController::jointCmdCallback, this, ros::TransportHints().reliable().tcpNoDelay()); publisher_controller_states_.init(node_handle, "/franka_ros_interface/motion_controller/arm/joint_controller_states", 1); { std::lock_guard > lock( publisher_controller_states_); publisher_controller_states_.msg_.controller_name = "effort_joint_torque_controller"; publisher_controller_states_.msg_.names.resize(joint_limits_.joint_names.size()); publisher_controller_states_.msg_.joint_controller_states.resize(joint_limits_.joint_names.size()); } return true; } void EffortJointTorqueController::starting(const ros::Time& /*time*/) { std::fill(jnt_cmd_.begin(), jnt_cmd_.end(), 0); prev_jnt_cmd_ = jnt_cmd_; ROS_WARN_STREAM("EffortJointTorqueController: Using raw torque controller! Be extremely careful and send smooth commands."); } void EffortJointTorqueController::update(const ros::Time& time, const ros::Duration& period) { std::array coriolis = model_handle_->getCoriolis(); std::array compensated_cmd{}; for (size_t i = 0; i < 7; ++i) { compensated_cmd[i] = coriolis_factor_ * coriolis[i] + jnt_cmd_[i]; } // Maximum torque difference with a sampling rate of 1 kHz. The maximum torque rate is // 1000 * (1 / sampling_time). std::array tau_d_saturated = saturateTorqueRate(compensated_cmd, prev_jnt_cmd_); if (trigger_publish_() && publisher_controller_states_.trylock()) { for (size_t i = 0; i < 7; ++i){ publisher_controller_states_.msg_.joint_controller_states[i].set_point = jnt_cmd_[i]; publisher_controller_states_.msg_.joint_controller_states[i].process_value = compensated_cmd[i]; publisher_controller_states_.msg_.joint_controller_states[i].time_step = period.toSec(); publisher_controller_states_.msg_.joint_controller_states[i].command = tau_d_saturated[i]; publisher_controller_states_.msg_.joint_controller_states[i].header.stamp = time; } publisher_controller_states_.unlockAndPublish(); } for (size_t i = 0; i < 7; ++i) { joint_handles_[i].setCommand(tau_d_saturated[i]); prev_jnt_cmd_[i] = tau_d_saturated[i]; } } bool EffortJointTorqueController::checkTorqueLimits(std::vector torques) { for (size_t i = 0; i < 7; ++i){ if (!(abs(torques[i]) < joint_limits_.effort[i])){ return true; } } return false; } std::array EffortJointTorqueController::saturateTorqueRate( const std::array& tau_d_calculated, const std::array& prev_tau) { // NOLINT (readability-identifier-naming) std::array tau_d_saturated{}; for (size_t i = 0; i < 7; i++) { double difference = tau_d_calculated[i] - prev_tau[i]; tau_d_saturated[i] = prev_tau[i] + std::max(std::min(difference, kDeltaTauMax), -kDeltaTauMax); } return tau_d_saturated; } void EffortJointTorqueController::jointCmdCallback(const franka_core_msgs::JointCommandConstPtr& msg) { if (msg->mode == franka_core_msgs::JointCommand::TORQUE_MODE){ if (msg->effort.size() != 7) { ROS_ERROR_STREAM( "EffortJointTorqueController: Published Commands are not of size 7"); std::fill(jnt_cmd_.begin(), jnt_cmd_.end(), 0); jnt_cmd_= prev_jnt_cmd_; } else if (checkTorqueLimits(msg->effort)) { ROS_ERROR_STREAM( "EffortJointTorqueController: Commanded torques are beyond allowed torque limits."); std::fill(jnt_cmd_.begin(), jnt_cmd_.end(), 0); jnt_cmd_= prev_jnt_cmd_; } else { std::copy_n(msg->effort.begin(), 7, jnt_cmd_.begin()); } } // else ROS_ERROR_STREAM("EffortJointTorqueController: Published Command msg are not of JointCommand::TORQUE_MODE! Dropping message"); } } // namespace franka_ros_controllers PLUGINLIB_EXPORT_CLASS(franka_ros_controllers::EffortJointTorqueController, controller_interface::ControllerBase) ================================================ FILE: franka_ros_controllers/src/position_joint_position_controller.cpp ================================================ /*************************************************************************** * * @package: franka_ros_controllers * @metapackage: franka_ros_interface * @author: Saif Sidhik * **************************************************************************/ /*************************************************************************** * Copyright (c) 2019-2021, Saif Sidhik. * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. **************************************************************************/ #include #include #include #include #include #include #include namespace franka_ros_controllers { bool PositionJointPositionController::init(hardware_interface::RobotHW* robot_hardware, ros::NodeHandle& node_handle) { desired_joints_subscriber_ = node_handle.subscribe( "/franka_ros_interface/motion_controller/arm/joint_commands", 20, &PositionJointPositionController::jointPosCmdCallback, this, ros::TransportHints().reliable().tcpNoDelay()); position_joint_interface_ = robot_hardware->get(); if (position_joint_interface_ == nullptr) { ROS_ERROR( "PositionJointPositionController: Error getting position joint interface from hardware!"); return false; } if (!node_handle.getParam("/robot_config/joint_names", joint_limits_.joint_names)) { ROS_ERROR("PositionJointPositionController: Could not parse joint names"); } if (joint_limits_.joint_names.size() != 7) { ROS_ERROR_STREAM("PositionJointPositionController: Wrong number of joint names, got " << joint_limits_.joint_names.size() << " instead of 7 names!"); return false; } std::map pos_limit_lower_map; std::map pos_limit_upper_map; if (!node_handle.getParam("/robot_config/joint_config/joint_position_limit/lower", pos_limit_lower_map) ) { ROS_ERROR( "PositionJointPositionController: Joint limits parameters not provided, aborting " "controller init!"); return false; } if (!node_handle.getParam("/robot_config/joint_config/joint_position_limit/upper", pos_limit_upper_map) ) { ROS_ERROR( "PositionJointPositionController: Joint limits parameters not provided, aborting " "controller init!"); return false; } for (size_t i = 0; i < joint_limits_.joint_names.size(); ++i){ if (pos_limit_lower_map.find(joint_limits_.joint_names[i]) != pos_limit_lower_map.end()) { joint_limits_.position_lower.push_back(pos_limit_lower_map[joint_limits_.joint_names[i]]); } else { ROS_ERROR("PositionJointPositionController: Unable to find lower position limit values for joint %s...", joint_limits_.joint_names[i].c_str()); } if (pos_limit_upper_map.find(joint_limits_.joint_names[i]) != pos_limit_upper_map.end()) { joint_limits_.position_upper.push_back(pos_limit_upper_map[joint_limits_.joint_names[i]]); } else { ROS_ERROR("PositionJointPositionController: Unable to find upper position limit values for joint %s...", joint_limits_.joint_names[i].c_str()); } } position_joint_handles_.resize(7); for (size_t i = 0; i < 7; ++i) { try { position_joint_handles_[i] = position_joint_interface_->getHandle(joint_limits_.joint_names[i]); } catch (const hardware_interface::HardwareInterfaceException& e) { ROS_ERROR_STREAM( "PositionJointPositionController: Exception getting joint handles: " << e.what()); return false; } } double controller_state_publish_rate(30.0); if (!node_handle.getParam("controller_state_publish_rate", controller_state_publish_rate)) { ROS_INFO_STREAM("PositionJointPositionController: Did not find controller_state_publish_rate. Using default " << controller_state_publish_rate << " [Hz]."); } trigger_publish_ = franka_hw::TriggerRate(controller_state_publish_rate); dynamic_reconfigure_joint_controller_params_node_ = ros::NodeHandle("/franka_ros_interface/position_joint_position_controller/arm/controller_parameters_config"); dynamic_server_joint_controller_params_ = std::make_unique< dynamic_reconfigure::Server>( dynamic_reconfigure_joint_controller_params_node_); dynamic_server_joint_controller_params_->setCallback( boost::bind(&PositionJointPositionController::jointControllerParamCallback, this, _1, _2)); publisher_controller_states_.init(node_handle, "/franka_ros_interface/motion_controller/arm/joint_controller_states", 1); { std::lock_guard > lock( publisher_controller_states_); publisher_controller_states_.msg_.controller_name = "position_joint_position_controller"; publisher_controller_states_.msg_.names.resize(joint_limits_.joint_names.size()); publisher_controller_states_.msg_.joint_controller_states.resize(joint_limits_.joint_names.size()); } return true; } void PositionJointPositionController::starting(const ros::Time& /* time */) { for (size_t i = 0; i < 7; ++i) { initial_pos_[i] = position_joint_handles_[i].getPosition(); } pos_d_ = initial_pos_; prev_pos_ = initial_pos_; pos_d_target_ = initial_pos_; } void PositionJointPositionController::update(const ros::Time& time, const ros::Duration& period) { for (size_t i = 0; i < 7; ++i) { position_joint_handles_[i].setCommand(pos_d_[i]); } double filter_val = filter_joint_pos_ * filter_factor_; for (size_t i = 0; i < 7; ++i) { prev_pos_[i] = position_joint_handles_[i].getPosition(); pos_d_[i] = filter_val * pos_d_target_[i] + (1.0 - filter_val) * pos_d_[i]; } if (trigger_publish_() && publisher_controller_states_.trylock()) { for (size_t i = 0; i < 7; ++i){ publisher_controller_states_.msg_.joint_controller_states[i].set_point = pos_d_target_[i]; publisher_controller_states_.msg_.joint_controller_states[i].process_value = pos_d_[i]; publisher_controller_states_.msg_.joint_controller_states[i].time_step = period.toSec(); publisher_controller_states_.msg_.joint_controller_states[i].header.stamp = time; } publisher_controller_states_.unlockAndPublish(); } // update parameters changed online either through dynamic reconfigure or through the interactive // target by filtering filter_joint_pos_ = param_change_filter_ * target_filter_joint_pos_ + (1.0 - param_change_filter_) * filter_joint_pos_; } bool PositionJointPositionController::checkPositionLimits(std::vector positions) { // bool retval = true; for (size_t i = 0; i < 7; ++i){ if (!((positions[i] <= joint_limits_.position_upper[i]) && (positions[i] >= joint_limits_.position_lower[i]))){ return true; } } return false; } void PositionJointPositionController::jointPosCmdCallback(const franka_core_msgs::JointCommandConstPtr& msg) { if (msg->mode == franka_core_msgs::JointCommand::POSITION_MODE){ if (msg->position.size() != 7) { ROS_ERROR_STREAM( "PositionJointPositionController: Published Commands are not of size 7"); pos_d_ = prev_pos_; pos_d_target_ = prev_pos_; } else if (checkPositionLimits(msg->position)) { ROS_ERROR_STREAM( "PositionJointPositionController: Commanded positions are beyond allowed position limits."); pos_d_ = prev_pos_; pos_d_target_ = prev_pos_; } else { std::copy_n(msg->position.begin(), 7, pos_d_target_.begin()); } } // else ROS_ERROR_STREAM("PositionJointPositionController: Published Command msg are not of JointCommand::POSITION_MODE! Dropping message"); } void PositionJointPositionController::jointControllerParamCallback(franka_ros_controllers::joint_controller_paramsConfig& config, uint32_t level){ target_filter_joint_pos_ = config.position_joint_delta_filter; } } // namespace franka_ros_controllers PLUGINLIB_EXPORT_CLASS(franka_ros_controllers::PositionJointPositionController, controller_interface::ControllerBase) ================================================ FILE: franka_ros_controllers/src/velocity_joint_velocity_controller.cpp ================================================ /*************************************************************************** * * @package: franka_ros_controllers * @metapackage: franka_ros_interface * @author: Saif Sidhik * **************************************************************************/ /*************************************************************************** * Copyright (c) 2019-2021, Saif Sidhik. * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. **************************************************************************/ #include #include #include #include #include #include #include namespace franka_ros_controllers { bool VelocityJointVelocityController::init(hardware_interface::RobotHW* robot_hardware, ros::NodeHandle& node_handle) { desired_joints_subscriber_ = node_handle.subscribe( "/franka_ros_interface/motion_controller/arm/joint_commands", 20, &VelocityJointVelocityController::jointVelCmdCallback, this, ros::TransportHints().reliable().tcpNoDelay()); velocity_joint_interface_ = robot_hardware->get(); if (velocity_joint_interface_ == nullptr) { ROS_ERROR( "VelocityJointVelocityController: Error getting velocity joint interface from hardware!"); return false; } if (!node_handle.getParam("/robot_config/joint_names", joint_limits_.joint_names)) { ROS_ERROR("VelocityJointVelocityController: Could not parse joint names"); } if (joint_limits_.joint_names.size() != 7) { ROS_ERROR_STREAM("VelocityJointVelocityController: Wrong number of joint names, got " << joint_limits_.joint_names.size() << " instead of 7 names!"); return false; } std::map vel_limit_map; if (!node_handle.getParam("/robot_config/joint_config/joint_velocity_limit", vel_limit_map) ) { ROS_ERROR( "VelocityJointVelocityController: Joint limits parameters not provided, aborting " "controller init!"); return false; } for (size_t i = 0; i < joint_limits_.joint_names.size(); ++i){ if (vel_limit_map.find(joint_limits_.joint_names[i]) != vel_limit_map.end()) { joint_limits_.velocity.push_back(vel_limit_map[joint_limits_.joint_names[i]]); } else { ROS_ERROR("VelocityJointVelocityController: Unable to find lower velocity limit values for joint %s...", joint_limits_.joint_names[i].c_str()); } } velocity_joint_handles_.resize(7); for (size_t i = 0; i < 7; ++i) { try { velocity_joint_handles_[i] = velocity_joint_interface_->getHandle(joint_limits_.joint_names[i]); } catch (const hardware_interface::HardwareInterfaceException& e) { ROS_ERROR_STREAM( "VelocityJointVelocityController: Exception getting joint handles: " << e.what()); return false; } } double controller_state_publish_rate(30.0); if (!node_handle.getParam("controller_state_publish_rate", controller_state_publish_rate)) { ROS_INFO_STREAM("VelocityJointVelocityController: Did not find controller_state_publish_rate. Using default " << controller_state_publish_rate << " [Hz]."); } trigger_publish_ = franka_hw::TriggerRate(controller_state_publish_rate); dynamic_reconfigure_joint_controller_params_node_ = ros::NodeHandle("/franka_ros_interface/velocity_joint_velocity_controller/arm/controller_parameters_config"); dynamic_server_joint_controller_params_ = std::make_unique< dynamic_reconfigure::Server>( dynamic_reconfigure_joint_controller_params_node_); dynamic_server_joint_controller_params_->setCallback( boost::bind(&VelocityJointVelocityController::jointControllerParamCallback, this, _1, _2)); publisher_controller_states_.init(node_handle, "/franka_ros_interface/motion_controller/arm/joint_controller_states", 1); { std::lock_guard > lock( publisher_controller_states_); publisher_controller_states_.msg_.controller_name = "velocity_joint_velocity_controller"; publisher_controller_states_.msg_.names.resize(joint_limits_.joint_names.size()); publisher_controller_states_.msg_.joint_controller_states.resize(joint_limits_.joint_names.size()); } return true; } void VelocityJointVelocityController::starting(const ros::Time& /* time */) { for (size_t i = 0; i < 7; ++i) { initial_vel_[i] = velocity_joint_handles_[i].getVelocity(); } vel_d_ = initial_vel_; prev_d_ = vel_d_; } void VelocityJointVelocityController::update(const ros::Time& time, const ros::Duration& period) { for (size_t i = 0; i < 7; ++i) { velocity_joint_handles_[i].setCommand(vel_d_[i]); } double filter_val = filter_joint_vel_ * filter_factor_; for (size_t i = 0; i < 7; ++i) { prev_d_[i] = velocity_joint_handles_[i].getVelocity(); vel_d_[i] = filter_val * vel_d_target_[i] + (1.0 - filter_val) * vel_d_[i]; } if (trigger_publish_() && publisher_controller_states_.trylock()) { for (size_t i = 0; i < 7; ++i){ publisher_controller_states_.msg_.joint_controller_states[i].set_point = vel_d_target_[i]; publisher_controller_states_.msg_.joint_controller_states[i].process_value = vel_d_[i]; publisher_controller_states_.msg_.joint_controller_states[i].time_step = period.toSec(); publisher_controller_states_.msg_.joint_controller_states[i].header.stamp = time; } publisher_controller_states_.unlockAndPublish(); } // update parameters changed online either through dynamic reconfigure or through the interactive // target by filtering filter_joint_vel_ = param_change_filter_ * target_filter_joint_vel_ + (1.0 - param_change_filter_) * filter_joint_vel_; } bool VelocityJointVelocityController::checkVelocityLimits(std::vector velocities) { // bool retval = true; for (size_t i = 0; i < 7; ++i){ if (!(abs(velocities[i]) <= joint_limits_.velocity[i])){ return true; } } return false; } void VelocityJointVelocityController::jointVelCmdCallback(const franka_core_msgs::JointCommandConstPtr& msg) { if (msg->mode == franka_core_msgs::JointCommand::VELOCITY_MODE){ if (msg->velocity.size() != 7) { ROS_ERROR_STREAM( "VelocityJointVelocityController: Published Commands are not of size 7"); vel_d_ = prev_d_; vel_d_target_ = prev_d_; } else if (checkVelocityLimits(msg->velocity)) { ROS_ERROR_STREAM( "VelocityJointVelocityController: Commanded velocities are beyond allowed velocity limits."); vel_d_ = prev_d_; vel_d_target_ = prev_d_; } else { std::copy_n(msg->velocity.begin(), 7, vel_d_target_.begin()); } } // else ROS_ERROR_STREAM("VelocityJointVelocityController: Published Command msg are not of JointCommand::Velocity! Dropping message"); } void VelocityJointVelocityController::jointControllerParamCallback(franka_ros_controllers::joint_controller_paramsConfig& config, uint32_t level){ target_filter_joint_vel_ = config.velocity_joint_delta_filter; } void VelocityJointVelocityController::stopping(const ros::Time& /*time*/) { // WARNING: DO NOT SEND ZERO VELOCITIES HERE AS IN CASE OF ABORTING DURING MOTION // A JUMP TO ZERO WILL BE COMMANDED PUTTING HIGH LOADS ON THE ROBOT. LET THE DEFAULT // BUILT-IN STOPPING BEHAVIOR SLOW DOWN THE ROBOT. } } // namespace franka_ros_controllers PLUGINLIB_EXPORT_CLASS(franka_ros_controllers::VelocityJointVelocityController, controller_interface::ControllerBase) ================================================ FILE: franka_ros_interface/CMakeLists.txt ================================================ cmake_minimum_required(VERSION 2.8.3) project(franka_ros_interface) find_package(catkin REQUIRED) catkin_metapackage() ================================================ FILE: franka_ros_interface/package.xml ================================================ franka_ros_interface 0.7.1 franka_ros_interface metapackage containing all of the SDK packages for Franka Panda robots using libfranka and franka-ros. Saif Sidhik Apache 2.0 Saif Sidhik catkin franka_interface franka_moveit franka_msgs franka_ros_controllers franka_tools ================================================ FILE: franka_tools/CMakeLists.txt ================================================ cmake_minimum_required(VERSION 2.8.3) project(franka_tools) ## Find catkin macros and libraries ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) ## is used, also find other catkin packages find_package(catkin REQUIRED COMPONENTS roscpp rospy std_msgs franka_control franka_interface franka_core_msgs ) ## System dependencies are found with CMake's conventions # find_package(Boost REQUIRED COMPONENTS system) catkin_python_setup() # include_directories(include) include_directories( ${catkin_INCLUDE_DIRS} ) ================================================ FILE: franka_tools/README.md ================================================ # franka_tools Some helper classes for controlling and handling the Franka Emika Panda robot. ## FrankaControllerManagerInterface - List, start, stop, load available controllers for the robot - Get the current controller status (commands, set points, controller gains, etc.) - Update controller parameters through *ControllerParamConfigClient* (see below) ## FrankaFramesInterface - Get and Set end-effector frame and stiffness frame of the robot easily - Set the frames to known frames (such as links on the robot) directly ## ControllerParamConfigClient - Get and set the controller parameters (gains) for the active controller ## JointTrajectoryActionClient - Command robot to given joint position(s) smoothly. (Uses the FollowJointTrajectory service from ROS *control_msgs* package) - Smoothly move to a desired (valid) pose without having to interpolate for smoothness (trajectory interpolation done internally) ## CollisionBehaviourInterface - Define collision and contact thresholds for the robot safety and contact detection. ================================================ FILE: franka_tools/package.xml ================================================ franka_tools 0.7.1 The franka_tools package Saif Sidhik Apache 2.0 Saif Sidhik catkin roscpp rospy std_msgs franka_interface franka_core_msgs franka_control roscpp rospy std_msgs franka_interface franka_core_msgs franka_control ================================================ FILE: franka_tools/setup.py ================================================ #!/usr/bin/env python from distutils.core import setup from catkin_pkg.python_setup import generate_distutils_setup d = generate_distutils_setup() d['packages'] = ['franka_tools'] d['package_dir'] = {'': 'src'} setup(**d) ================================================ FILE: franka_tools/src/franka_tools/__init__.py ================================================ # /*************************************************************************** # Copyright (c) 2019-2021, Saif Sidhik # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. # You may obtain a copy of the License at # http://www.apache.org/licenses/LICENSE-2.0 # Unless required by applicable law or agreed to in writing, software # distributed under the License is distributed on an "AS IS" BASIS, # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. # See the License for the specific language governing permissions and # limitations under the License. # **************************************************************************/ from .controller_param_config_client import ControllerParamConfigClient from .frames_interface import FrankaFramesInterface from .joint_trajectory_action_client import JointTrajectoryActionClient from .controller_manager_interface import FrankaControllerManagerInterface from .collision_behaviour_interface import CollisionBehaviourInterface ================================================ FILE: franka_tools/src/franka_tools/collision_behaviour_interface.py ================================================ # /*************************************************************************** # # @package: franka_tools # @metapackage: franka_ros_interface # @author: Saif Sidhik # # **************************************************************************/ # /*************************************************************************** # Copyright (c) 2019-2021, Saif Sidhik # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. # You may obtain a copy of the License at # http://www.apache.org/licenses/LICENSE-2.0 # Unless required by applicable law or agreed to in writing, software # distributed under the License is distributed on an "AS IS" BASIS, # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. # See the License for the specific language governing permissions and # limitations under the License. # **************************************************************************/ import rospy from franka_msgs.srv import SetForceTorqueCollisionBehavior DEFAULT_VALUES = { 'torque_acc_lower' : [20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0], 'torque_acc_upper' : [20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0], 'torque_lower' : [20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0], 'torque_upper' : [20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0], 'force_acc_lower' : [20.0, 20.0, 20.0, 25.0, 25.0, 25.0], 'force_acc_upper' : [20.0, 20.0, 20.0, 25.0, 25.0, 25.0], 'force_lower' : [5.0, 5.0, 5.0, 25.0, 25.0, 25.0], '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. } class CollisionBehaviourInterface(object): """ Helper class to set collision and contact thresholds at cartesian and joint levels. (This class has no 'getter' functions to access the currently set collision behaviour valures.) """ def __init__(self): rospy.loginfo("Waiting for collision behaviour services...") rospy.wait_for_service("/franka_ros_interface/franka_control/set_force_torque_collision_behavior", timeout = 1.0) self._ft_collision_behaviour_handle = rospy.ServiceProxy("/franka_ros_interface/franka_control/set_force_torque_collision_behavior", SetForceTorqueCollisionBehavior) self._full_collision_behaviour_handle = rospy.ServiceProxy("/franka_ros_interface/franka_control/set_full_collision_behavior", SetForceTorqueCollisionBehavior) rospy.loginfo("Collision behaviour services found.") def set_ft_contact_collision_behaviour(self, torque_lower = None, torque_upper = None, force_lower = None, force_upper = None): """ :return: True if service call successful, False otherwise :rtype: bool :param torque_lower: Joint torque threshold for contact detection :type torque_lower: [float] size 7 :param torque_upper: Joint torque threshold for collision (robot motion stops if violated) :type torque_upper: [float] size 7 :param force_lower: Cartesian force threshold for contact detection [x,y,z,R,P,Y] :type force_lower: [float] size 6 :param force_upper: Cartesian force threshold for collision detection [x,y,z,R,P,Y] (robot motion stops if violated) :type force_upper: [float] size 6 """ if torque_lower is None: torque_lower = DEFAULT_VALUES['torque_lower'] if torque_upper is None: torque_upper = DEFAULT_VALUES['torque_upper'] if force_lower is None: force_lower = DEFAULT_VALUES['force_lower'] if force_upper is None: force_upper = DEFAULT_VALUES['force_upper'] try: response = self._ft_collision_behaviour_handle(lower_torque_thresholds_nominal = torque_lower, upper_torque_thresholds_nominal = torque_upper, lower_force_thresholds_nominal = force_lower, upper_force_thresholds_nominal = force_upper) rospy.loginfo("CollisionBehaviourInterface: Collision behaviour change request: %s. \n\tDetails: %s"%("Success" if response.success else "Failed!", response.error)) except rospy.ServiceException as e: rospy.logwarn("CollisionBehaviourInterface: Collision behaviour change service call failed: %s"%e) return False def set_force_threshold_for_contact(self, cartesian_force_values): """ :return: True if service call successful, False otherwise :rtype: bool :param cartesian_force_values: Cartesian force threshold for contact detection [x,y,z,R,P,Y] :type cartesian_force_values: [float] size 6 """ return self.set_ft_contact_collision_behaviour(force_lower = cartesian_force_values) def set_force_threshold_for_collision(self, cartesian_force_values): """ :return: True if service call successful, False otherwise :rtype: bool :param cartesian_force_values: Cartesian force threshold for collision detection [x,y,z,R,P,Y] (robot motion stops if violated) :type cartesian_force_values: [float] size 6 """ return self.set_ft_contact_collision_behaviour(force_upper = cartesian_force_values) def set_collision_threshold(self, joint_torques = None, cartesian_forces = None): """ :return: True if service call successful, False otherwise :rtype: bool :param joint_torques: Joint torque threshold for collision (robot motion stops if violated) :type joint_torques: [float] size 7 :param cartesian_forces: Cartesian force threshold for collision detection [x,y,z,R,P,Y] (robot motion stops if violated) :type cartesian_forces: [float] size 6 """ return self.set_ft_contact_collision_behaviour(torque_upper = joint_torques, force_upper = cartesian_forces) def set_contact_threshold(self, joint_torques = None, cartesian_forces = None): """ :return: True if service call successful, False otherwise :rtype: bool :param joint_torques: Joint torque threshold for identifying as contact :type joint_torques: [float] size 7 :param cartesian_forces: Cartesian force threshold for identifying as contact :type cartesian_forces: [float] size 6 """ return self.set_ft_contact_collision_behaviour(torque_lower = joint_torques, force_lower = cartesian_forces) ================================================ FILE: franka_tools/src/franka_tools/controller_manager_interface.py ================================================ # /*************************************************************************** # # @package: franka_tools # @metapackage: franka_ros_interface # @author: Saif Sidhik # # **************************************************************************/ # /*************************************************************************** # Copyright (c) 2019-2021, Saif Sidhik # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. # You may obtain a copy of the License at # http://www.apache.org/licenses/LICENSE-2.0 # Unless required by applicable law or agreed to in writing, software # distributed under the License is distributed on an "AS IS" BASIS, # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. # See the License for the specific language governing permissions and # limitations under the License. # **************************************************************************/ import rospy import numpy as np from copy import deepcopy from controller_manager_msgs.msg import ControllerState from controller_manager_msgs.srv import * import socket from franka_core_msgs.msg import JointControllerStates from franka_tools import ControllerParamConfigClient from controller_manager_msgs.utils import ( ControllerLister, get_rosparam_controller_names) def _resolve_controllers_ns(cm_ns): """ Resolve the namespace containing controller configurations from that of the controller manager. Controllers are assumed to live one level above the controller manager, e.g. >>> _resolve_controller_ns('/path/to/controller_manager') '/path/to' In the particular case in which the controller manager is not namespaced, the controller is assumed to live in the root namespace >>> _resolve_controller_ns('/') '/' >>> _resolve_controller_ns('') '/' :param cm_ns Controller manager namespace (can be an empty string) :type cm_ns str :return: Controllers namespace :rtype: str """ ns = cm_ns.rsplit('/', 1)[0] if not ns: ns += '/' return ns def _append_ns(in_ns, suffix): """ Append a sub-namespace (suffix) to the input namespace :param in_ns Input namespace :type in_ns str :return: Suffix namespace :rtype: str """ ns = in_ns if ns[-1] != '/': ns += '/' ns += suffix return ns def _rosparam_controller_type(ctrls_ns, ctrl_name): """ Get a controller's type from its ROS parameter server configuration :param ctrls_ns Namespace where controllers should be located :type ctrls_ns str :param ctrl_name Controller name :type ctrl_name str :return: Controller type :rtype: str """ type_param = _append_ns(ctrls_ns, ctrl_name) + '/type' return rospy.get_param(type_param) def _get_controller_name_from_rosparam_server(rosparam_name): try: cname = rospy.get_param(rosparam_name) except KeyError: rospy.loginfo("FrankaControllerManagerInterface: cannot detect controller name" " under param {}".format(rosparam_name)) cname = '' except (socket.error, socket.gaierror): rospy.logerr("Failed to connect to the ROS parameter server!\n" "Please check to make sure your ROS networking is " "properly configured:\n") sys.exit() return cname class ControllerStateInfo: """ Class for creating a storage object for controller state published by the induvidual franka_ros_controllers. """ def __init__(self, controller_state_msg): self.controller_name = controller_state_msg.controller_name self.p = np.asarray([j.p for j in controller_state_msg.joint_controller_states]) self.d = np.asarray([j.d for j in controller_state_msg.joint_controller_states]) self.i = np.asarray([j.i for j in controller_state_msg.joint_controller_states]) self.process_value = np.asarray([j.process_value for j in controller_state_msg.joint_controller_states]) self.set_point = np.asarray([j.set_point for j in controller_state_msg.joint_controller_states]) self.process_value_dot = np.asarray([j.process_value_dot for j in controller_state_msg.joint_controller_states]) self.error = np.asarray([j.error for j in controller_state_msg.joint_controller_states]) self.time_step = np.asarray([j.time_step for j in controller_state_msg.joint_controller_states]) self.i_clamp = np.asarray([j.i_clamp for j in controller_state_msg.joint_controller_states]) self.command = np.asarray([j.command for j in controller_state_msg.joint_controller_states]) class FrankaControllerManagerInterface(object): """ :type synchronous_pub: bool :param synchronous_pub: designates the JointCommand Publisher as Synchronous if True and Asynchronous if False. Synchronous Publishing means that all joint_commands publishing to the robot's joints will block until the message has been serialized into a buffer and that buffer has been written to the transport of every current Subscriber. This yields predicable and consistent timing of messages being delivered from this Publisher. However, when using this mode, it is possible for a blocking Subscriber to prevent the joint_command functions from exiting. Unless you need exact JointCommand timing, default to Asynchronous Publishing (False). :param ns: base namespace of interface ('frank_ros_interface'/'panda_simulator') :type ns: str :param sim: Flag specifying whether the robot is in simulation or not (can be obtained from :py:class:`franka_interface.RobotParams` instance) :type sim: bool """ def __init__(self, ns="franka_ros_interface", synchronous_pub = False, sim = False): self._ns = ns if ns[0] == '/' else '/' + ns self._prefix = "/controller_manager" self._cm_ns = self._prefix self._service_names = ["list_controllers", "unload_controller", "load_controller", "switch_controller"] load_srv_name = self._cm_ns + "/load_controller" self._load_srv = rospy.ServiceProxy(load_srv_name, LoadController, persistent=True) unload_srv_name = self._cm_ns + "/unload_controller" self._unload_srv = rospy.ServiceProxy(unload_srv_name, UnloadController, persistent=True) switch_srv_name = self._cm_ns + "/switch_controller" self._switch_srv = rospy.ServiceProxy(switch_srv_name, SwitchController, persistent=True) list_srv_name = self._cm_ns + "/list_controllers" self._list_srv = rospy.ServiceProxy(list_srv_name, ListControllers, persistent=True) list_types_srv_name = self._cm_ns + "/list_controller_types" self._list_types_srv = rospy.ServiceProxy(list_types_srv_name, ListControllerTypes, persistent=True) self._in_sim = sim self._controller_lister = ControllerLister(self._cm_ns) self._controller_names_from_rosparam = { 'joint_position_controller': _get_controller_name_from_rosparam_server('/controllers_config/position_controller'), 'joint_velocity_controller': _get_controller_name_from_rosparam_server('/controllers_config/velocity_controller'), 'joint_torque_controller': _get_controller_name_from_rosparam_server('/controllers_config/torque_controller'), 'joint_trajectory_controller': _get_controller_name_from_rosparam_server('/controllers_config/trajectory_controller'), 'effort_joint_position_controller': _get_controller_name_from_rosparam_server('/controllers_config/impedance_controller'), 'default_controller': _get_controller_name_from_rosparam_server('/controllers_config/default_controller'), } if self._in_sim: 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'] else: self._non_motion_controllers = [self._ns[1:] + '/custom_franka_state_controller',self._ns[1:] + '/franka_state_controller'] self._assert_one_active_controller() self._state_subscriber = rospy.Subscriber("%s/motion_controller/arm/joint_controller_states" %(self._ns), JointControllerStates, self._on_controller_state, queue_size = 1, tcp_nodelay = True) self._param_config_clients = {} self._dont_start_config_client = False rospy.on_shutdown(self._clean_shutdown) def _clean_shutdown(self): if self._state_subscriber: self._state_subscriber.unregister() def _on_controller_state(self, msg): self._controller_state = deepcopy(ControllerStateInfo(msg)) self._assert_one_active_controller() if self._current_controller and self._current_controller not in self._param_config_clients: self._param_config_clients[self._current_controller] = ControllerParamConfigClient(self._current_controller) self._param_config_clients[self._current_controller].start() def _assert_one_active_controller(self): ctrlr_list = self.list_active_controllers(only_motion_controllers=True) assert len(ctrlr_list) <= 1, "FrankaControllerManagerInterface: More than one motion controller active!" self._current_controller = ctrlr_list[0].name if len(ctrlr_list) > 0 else None def load_controller(self, name): """ Loads the specified controller :type name: str :param name: name of the controller to be loaded """ self._load_srv.call(LoadControllerRequest(name=name)) def unload_controller(self, name): """ Unloads the specified controller :type name: str :param name: name of the controller to be unloaded """ self._unload_srv.call(UnloadControllerRequest(name=name)) def start_controller(self, name): """ Starts the specified controller :type name: str :param name: name of the controller to be started """ 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 strict = SwitchControllerRequest.STRICT req = SwitchControllerRequest(start_controllers=[name], stop_controllers=[], strictness=strict) rospy.logdebug("FrankaControllerManagerInterface: Starting controller: %s"%name) self._switch_srv.call(req) self._assert_one_active_controller() def get_controller_state(self): """ Get the status of the current controller, including set points, computed command, controller gains etc. See the ControllerStateInfo class (above) parameters for more info. """ return deepcopy(self._controller_state) def stop_controller(self, name): """ Stops the specified controller :type name: str :param name: name of the controller to be stopped """ strict = SwitchControllerRequest.STRICT req = SwitchControllerRequest(start_controllers=[], stop_controllers=[name], strictness=strict) rospy.logdebug("FrankaControllerManagerInterface: Stopping controller: %s"%name) self._switch_srv.call(req) def list_loaded_controllers(self): """ :return: List of controller types associated to a controller manager namespace. Contains all loaded controllers, as returned by the `list_controller_types` service, plus uninitialized controllers with configurations loaded in the parameter server. :rtype: [str] """ req = ListControllersRequest() return self._list_srv.call(req) def list_controller_types(self): """ :return: List of controller types associated to a controller manager namespace. Contains both stopped/running/loaded controllers, as returned by the `list_controller_types` service, plus uninitialized controllers with configurations loaded in the parameter server. :rtype: [str] """ req = ListControllerTypesRequest() return self._list_types_srv.call(req) def list_controllers(self): """ :return: List of controllers associated to a controller manager namespace. Contains both stopped/running controllers, as returned by the `list_controllers` service, plus uninitialized controllers with configurations loaded in the parameter server. :rtype: [ControllerState obj] """ if not self._cm_ns: return [] # Add loaded controllers first controllers = self._controller_lister() # Append potential controller configs found in the parameter server all_ctrls_ns = _resolve_controllers_ns(self._cm_ns) for name in get_rosparam_controller_names(all_ctrls_ns): add_ctrl = not any(name == ctrl.name for ctrl in controllers) if add_ctrl: type_str = _rosparam_controller_type(all_ctrls_ns, name) uninit_ctrl = ControllerState(name=name, type=type_str, state='uninitialized') controllers.append(uninit_ctrl) for c in controllers: if c.name[0] == '/': c.name = c.name[1:] return controllers def controller_dict(self): """ Get all controllers as dict :return: name of the controller to be stopped :rtype: dict {'controller_name': ControllerState} """ controllers = self.list_controllers() controller_dict = {} for c in controllers: controller_dict[c.name] = c return controller_dict def set_motion_controller(self, controller_name): """ Set the specified controller as the (only) motion controller :return: name of currently active controller (can be used to switch back to this later) :rtype: str :type controller_name: str :param controller_name: name of controller to start """ if controller_name.strip() == '': rospy.logwarn("FrankaControllerManagerInterface: Empty controller name in controller switch request. Ignoring.") return self._current_controller if controller_name[0] == '/': controller_name = controller_name[1:] curr_ctrlr = self._current_controller switch_ctrl = (curr_ctrlr != controller_name) if switch_ctrl: active_controllers = self.list_active_controllers(only_motion_controllers = True) for ctrlr in active_controllers: self.stop_controller(ctrlr.name) rospy.sleep(0.5) if not self.is_loaded(controller_name): self.load_controller(controller_name) self.start_controller(controller_name) else: rospy.logdebug("FrankaControllerManagerInterface: Controller '{0}' already active. Not switching.".format(controller_name)) return curr_ctrlr def is_running(self, controller_name): """ Check if the given controller is running. :type controller_name: str :param controller_name: name of controller whose status is to be checked :return: True if controller is running, False otherwise :rtype: bool """ controllers = self.controller_dict() ctrl_state = controllers.get(controller_name,None) return ctrl_state is not None and ctrl_state.state=="running" def is_loaded(self, controller_name): """ Check if the given controller is loaded. :type controller_name: str :param controller_name: name of controller whose status is to be checked :return: True if controller is loaded, False otherwise :rtype: bool """ controllers = self.controller_dict() ctrl_state = controllers.get(controller_name,None) return ctrl_state is not None and ctrl_state.state!="uninitialized" def list_motion_controllers(self): """ :return: List of motion controllers associated to a controller manager namespace. Contains both stopped/running controllers, as returned by the `list_controllers` service, plus uninitialized controllers with configurations loaded in the parameter server. :rtype: [ControllerState obj] """ motion_controllers = [] for controller in self.list_controllers(): if not controller.name in self._non_motion_controllers: motion_controllers.append(controller) return motion_controllers def list_active_controllers(self, only_motion_controllers = False): """ :return: List of active controllers associated to a controller manager namespace. Contains both stopped/running controllers, as returned by the `list_controllers` service, plus uninitialized controllers with configurations loaded in the parameter server. :rtype: [ControllerState obj] :param only_motion_controller: if True, only motion controllers are returned :type only_motion_controller: bool """ if only_motion_controllers: controllers = self.list_motion_controllers() else: controllers = self.list_controllers() active_controllers = [] for controller in controllers: if controller.state == "running": active_controllers.append(controller) return active_controllers def list_active_controller_names(self, only_motion_controllers = False): """ :return: List of names active controllers associated to a controller manager namespace. :rtype: [str] :param only_motion_controller: if True, only motion controllers are returned :type only_motion_controller: bool """ return [c.name for c in self.list_active_controllers(only_motion_controllers = only_motion_controllers)] def get_controller_config_client(self, controller_name): """ :return: The parameter configuration client object associated with the specified controller :rtype: ControllerParamConfigClient obj (if None, returns False) :param controller_name: name of controller whose config client is required :type controller_name: str """ if controller_name in self._param_config_clients: return self._param_config_clients[controller_name] else: rospy.logwarn("FrankaControllerManagerInterface: No parameter configuration client available for controller {}".format(controller_name)) return False def get_current_controller_config_client(self): """ :return: The parameter configuration client object associated with the currently active controller :rtype: ControllerParamConfigClient obj (if None, returns False) :param controller_name: name of controller whose config client is required :type controller_name: str """ if self._current_controller is None: rospy.logwarn("FrankaControllerManagerInterface: No active controller!") return False return self.get_controller_config_client(self._current_controller) def list_controller_names(self): """ :return: List of names all controllers associated to a controller manager namespace. :rtype: [str] :param only_motion_controller: if True, only motion controllers are returned :type only_motion_controller: bool """ return [c.name for c in self.list_controllers()] """ Properties that give the names of the controllers for each type. """ @property def joint_velocity_controller(self): """ :getter: Returns the name of joint velocity controller (defined in franka_ros_controllers, and specified in robot_config.yaml). Can be used for changing motion controller using :py:meth:`FrankaControllerManagerInterface.set_motion_controller`. :type: str """ return self._controller_names_from_rosparam['joint_velocity_controller'] @property def joint_position_controller(self): """ :getter: Returns the name of joint position controller (defined in franka_ros_controllers, and specified in robot_config.yaml). Can be used for changing motion controller using :py:meth:`FrankaControllerManagerInterface.set_motion_controller`. :type: str """ return self._controller_names_from_rosparam['joint_position_controller'] @property def joint_torque_controller(self): """ :getter: Returns the name of joint torque controller (defined in franka_ros_controllers, and specified in robot_config.yaml). Can be used for changing motion controller using :py:meth:`FrankaControllerManagerInterface.set_motion_controller`. :type: str """ return self._controller_names_from_rosparam['joint_torque_controller'] @property def joint_impedance_controller(self): """ :getter: Returns the name of joint impedance controller (defined in franka_ros_controllers, and specified in robot_config.yaml). Can be used for changing motion controller using :py:meth:`FrankaControllerManagerInterface.set_motion_controller`. :type: str """ return self._controller_names_from_rosparam['joint_impedance_controller'] @property def effort_joint_position_controller(self): """ :getter: Returns the name of effort-based joint position controller (defined in franka_ros_controllers, and specified in robot_config.yaml). Can be used for changing motion controller using :py:meth:`FrankaControllerManagerInterface.set_motion_controller`. :type: str """ return self._controller_names_from_rosparam['effort_joint_position_controller'] @property def joint_trajectory_controller(self): """ :getter: Returns the name of joint trajectory controller (defined in franka_ros_controllers, and specified in robot_config.yaml). Can be used for changing motion controller using :py:meth:`FrankaControllerManagerInterface.set_motion_controller`. This controller exposes trajectory following service. :type: str """ if self._in_sim: return self.joint_position_controller return self._controller_names_from_rosparam['joint_trajectory_controller'] @property def current_controller(self): """ :getter: Returns the name of currently active controller. :type: str """ self._assert_one_active_controller() if not self._current_controller: rospy.logwarn("FrankaControllerManagerInterface: No active controller!") return self._current_controller @property def default_controller(self): """ :getter: Returns the name of the default controller for Franka ROS Interface. (specified in robot_config.yaml). Should ideally default to the same as :py:meth:`joint_trajectory_controller`. :type: str """ return self._controller_names_from_rosparam['default_controller'] if __name__ == '__main__': from franka_tools import FrankaFramesInterface cmi = FrankaControllerManagerInterface() f = FrankaFramesInterface() ================================================ FILE: franka_tools/src/franka_tools/controller_param_config_client.py ================================================ # /*************************************************************************** # # @package: franka_tools # @metapackage: franka_ros_interface # @author: Saif Sidhik # # **************************************************************************/ # /*************************************************************************** # Copyright (c) 2019-2021, Saif Sidhik # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. # You may obtain a copy of the License at # http://www.apache.org/licenses/LICENSE-2.0 # Unless required by applicable law or agreed to in writing, software # distributed under the License is distributed on an "AS IS" BASIS, # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. # See the License for the specific language governing permissions and # limitations under the License. # **************************************************************************/ import rospy import dynamic_reconfigure.client K_GAINS_KW = ['j1_k','j2_k','j3_k','j4_k','j5_k','j6_k','j7_k'] D_GAINS_KW = ['j1_d','j2_d','j3_d','j4_d','j5_d','j6_d','j7_d'] class ControllerParamConfigClient: """ Interface class for updating dynamically configurable paramters of a controller. :param controller_name: The name of the controller. :type controller_name: str """ def __init__(self, controller_name): """ Initialisation: Client is not started yet. """ self._controller_name = controller_name if controller_name[0] != '/' else controller_name[1:] self._is_running = False @property def is_running(self): """ :return: True if client is running / server is unavailable; False otherwise :rtype: bool """ return self._is_running def start(self, timeout = 5): """ Start the dynamic_reconfigure client :param timeout: time to wait before giving up on service request :type timeout: float """ service_name = "/{}/arm/controller_parameters_config".format(self._controller_name) try: self._client = dynamic_reconfigure.client.Client(service_name, timeout=timeout, config_callback=self._log_update) except rospy.ROSException: rospy.logdebug("ControllerParamConfigClient: Could not find configuration server at {}".format(service_name)) self._is_running = True def _log_update(self, config): """ Optional callback to log parameter changes after each update request. """ k_gains = [config[n] for n in K_GAINS_KW] d_gains = [config[n] for n in D_GAINS_KW] 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)) def update_config(self, **kwargs): """ Update the config in the server using the provided keyword arguments. :param kwargs: These are keyword arguments matching the parameter names in config file: franka_ros_controllers/cfg/joint_controller_params.cfg """ self._client.update_configuration(kwargs) def set_controller_gains(self, k_gains, d_gains = None): """ Update the stiffness and damping parameters of the joints for the current controller. :param k_gains: joint stiffness parameters (should be within limits specified in franka documentation; same is also set in franka_ros_controllers/cfg/joint_controller_params.cfg) :type k_gains: [float] :param d_gains: joint damping parameters (should be within limits specified in franka documentation; same is also set in franka_ros_controllers/cfg/joint_controller_params.cfg) :type d_gains: [float] """ assert len(k_gains) == 7, "ControllerParamConfigClient: k_gains argument should be of length 7!" config = {} for i, k_val in enumerate(k_gains): config[K_GAINS_KW[i]] = k_val if d_gains: assert len(k_gains) == 7, "ControllerParamConfigClient: d_gains argument should be of length 7!" for j, d_val in enumerate(d_gains): config[D_GAINS_KW[j]] = d_val rospy.logdebug("Config: {}".format(config)) self.update_config(**config) def set_joint_motion_smoothing_parameter(self, value): """ Update the joint motion smoothing parameter (only valid for position_joint_position_controller). :param value: smoothing factor (should be within limit set in franka_ros_controllers/cfg/joint_controller_params.cfg) :type value: [float] """ self.update_config(position_joint_delta_filter = value) def get_joint_motion_smoothing_parameter(self, timeout = 5): """ :return: the currently set value for the joint position smoothing parameter from the server. :rtype: float :param timeout: time to wait before giving up on service request :type timeout: float """ return self.get_config(timeout = timeout)['position_joint_delta_filter'] def get_config(self, timeout = 5): """ :return: the currently set values for all paramters from the server :rtype: dict {str : float} :param timeout: time to wait before giving up on service request :type timeout: float """ return self._client.get_configuration(timeout = timeout) def get_controller_gains(self, timeout = 5): """ :return: the currently set values for controller gains from the server :rtype: ( [float], [float] ) :param timeout: time to wait before giving up on service request :type timeout: float """ config = self.get_config(timeout = timeout) k_gains = [] for k_val_ in K_GAINS_KW: if k_val_ in config: k_gains.append(config[k_val_]) else: rospy.logwarn("ControllerParamConfigClient: Could not find K gain {} in server".format(k_val_)) return False, False d_gains = [] for d_val_ in D_GAINS_KW: if d_val_ in config: d_gains.append(config[d_val_]) else: rospy.logwarn("ControllerParamConfigClient: Could not find D gain {} in server".format(d_val_)) return k_gains, False return k_gains, d_gains def get_parameter_descriptions(self, timeout = 5): """ :return: the description of each parameter as defined in the cfg file from the server. :rtype: dict {str : str} :param timeout: time to wait before giving up on service request :type timeout: float """ return self._client.get_parameter_descriptions(timeout = timeout) if __name__ == "__main__": rospy.init_node("dynamic_client") # client = dynamic_reconfigure.client.Client("dynamic_tutorials", timeout=30, config_callback=callback) ================================================ FILE: franka_tools/src/franka_tools/frames_interface.py ================================================ # /*************************************************************************** # # @package: franka_tools # @metapackage: franka_ros_interface # @author: Saif Sidhik # # **************************************************************************/ # /*************************************************************************** # Copyright (c) 2019-2021, Saif Sidhik # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. # You may obtain a copy of the License at # http://www.apache.org/licenses/LICENSE-2.0 # Unless required by applicable law or agreed to in writing, software # distributed under the License is distributed on an "AS IS" BASIS, # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. # See the License for the specific language governing permissions and # limitations under the License. # **************************************************************************/ import tf2_ros as tf import rospy import quaternion import numpy as np import franka_dataflow from franka_msgs.srv import SetEEFrame, SetKFrame from collections import namedtuple _FRAME_NAMES = namedtuple('Constants', ['EE_FRAME', 'K_FRAME']) DEFAULT_TRANSFORMATIONS = _FRAME_NAMES([1, 0.0, 0.0, 0.0, 0.0, 1, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0], # EE_FRAME [1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0] # K_FRAME ) # identity matrices by default class FrankaFramesInterface(object): """ Helper class to retrieve and set EE frames. .. 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. .. note:: All controllers have to be unloaded before switching frames. This has to be done externally (also automatically handled in ArmInterface class). .. 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. """ def __init__(self): self._current_EE_frame_transformation = None # NE_T_EE self._current_K_frame_transformation = None # EE_T_K def set_EE_frame(self, frame): """ Set new EE frame based on the transformation given by 'frame', which is the transformation matrix defining the new desired EE frame with respect to the nominal end-effector frame (NE_T_EE). :type frame: [float (16,)] / np.ndarray (4x4) :param frame: transformation matrix of new EE frame wrt nominal end-effector frame (if list, should be column major) :rtype: bool :return: success status of service request """ frame = self._assert_frame_validity(frame) return self._request_setEE_service(frame) def _update_frame_data(self, EE_frame_transformation, K_frame_transformation): """ The callback function used by the PandaArm class to update the current frame transformations NE_T_EE and EE_T_K. """ assert len( EE_frame_transformation) == 16, "FrankaFramesInterface: Current EE frame transformation could not be retrieved!" self._current_EE_frame_transformation = EE_frame_transformation assert len( K_frame_transformation) == 16, "FrankaFramesInterface: Current K frame transformation could not be retrieved!" self._current_K_frame_transformation = K_frame_transformation def _assert_frame_validity(self, frame): if isinstance(frame, np.ndarray): if frame.shape[0] == frame.shape[1] == 4: frame = frame.flatten('F').tolist() else: raise ValueError( "Invalid shape for transformation matrix numpy array") else: assert len( frame) == 16, "Invalid number of elements in transformation matrix. Should have 16 elements." return frame def get_link_tf(self, frame_name, timeout=5.0, parent='panda_NE'): """ Get :math:`4\times 4` transformation matrix of a frame with respect to another. :return: :math:`4\times 4` transformation matrix :rtype: np.ndarray :param frame_name: Name of the child frame from the TF tree :type frame_name: str :param parent: Name of parent frame (default: 'panda_NE', the default nominal end-effector frame set in Desk) :type parent: str """ tfBuffer = tf.Buffer() listener = tf.TransformListener(tfBuffer) err = "FrankaFramesInterface: Error while looking up transform from Flange frame to link frame %s" % frame_name def body(): try: tfBuffer.lookup_transform(parent, frame_name, rospy.Time(0)) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): return False return True franka_dataflow.wait_for( lambda: body(), timeout=timeout, raise_on_error=True, timeout_msg=err) trans_stamped = tfBuffer.lookup_transform( parent, frame_name, rospy.Time(0)) t = [trans_stamped.transform.translation.x, trans_stamped.transform.translation.y, trans_stamped.transform.translation.z] q = trans_stamped.transform.rotation rot = np.quaternion(q.w, q.x, q.y, q.z) rot = quaternion.as_rotation_matrix(rot) trans_mat = np.eye(4) trans_mat[:3, :3] = rot trans_mat[:3, 3] = np.array(t) return trans_mat def frames_are_same(self, frame1, frame2): """ :return: True if two transformation matrices are equal :rtype: bool :param frame1: 4x4 transformation matrix representing frame1 :type frame1: np.ndarray (shape: [4,4]), or list (flattened column major 4x4) :param frame2: 4x4 transformation matrix representing frame2 :type frame2: np.ndarray (shape: [4,4]), or list (flattened column major 4x4) """ frame1 = self._assert_frame_validity(frame1) frame2 = self._assert_frame_validity(frame2) return np.array_equal(np.asarray(frame1), np.asarray(frame2)) def EE_frame_already_set(self, frame): """ :return: True if the requested frame is already the current EE frame :rtype: bool :param frame: 4x4 transformation matrix representing frame :type frame: np.ndarray (shape: [4,4]), or list (flattened column major 4x4) """ return self.frames_are_same(frame, self._current_EE_frame_transformation) def set_EE_at_frame(self, frame_name, timeout=5.0): """ Set new EE frame to the same frame as the link frame given by 'frame_name'. Only 'panda_link7', 'panda_link8', 'panda_hand', 'panda_NE', and 'panda_EE' are valid frames for this operation. Other frames will not produce required results. Motion controllers should be stopped for switching. :type frame_name: str :param frame_name: desired tf frame name in the tf tree :rtype: [bool, str] :return: [success status of service request, error msg if any] """ return self.set_EE_frame(self.get_link_tf(frame_name, timeout)) def get_EE_frame(self, as_mat=False): """ Get current EE frame transformation matrix in flange frame :type as_mat: bool :param as_mat: if True, return np array, else as list :rtype: [float (16,)] / np.ndarray (4x4) :return: transformation matrix of EE frame wrt flange frame (column major) """ return self._current_EE_frame_transformation if not as_mat else np.asarray(self._current_EE_frame_transformation).reshape(4, 4, order='F') def reset_EE_frame(self): """ Reset EE frame to default. (defined by DEFAULT_TRANSFORMATIONS.EE_FRAME global variable defined above) By default, this is identity aligning it with the nominal end-effector frame. :rtype: bool :return: success status of service request """ return self.set_EE_frame(frame=DEFAULT_TRANSFORMATIONS.EE_FRAME) def EE_frame_is_reset(self): assert self._current_EE_frame_transformation is not None, "FrankaFramesInterface: Current EE Frame is not known." return list(self._current_EE_frame_transformation) == list(DEFAULT_TRANSFORMATIONS.EE_FRAME) def _request_setEE_service(self, trans_mat): rospy.wait_for_service( '/franka_ros_interface/franka_control/set_EE_frame') try: service_handle = rospy.ServiceProxy( '/franka_ros_interface/franka_control/set_EE_frame', SetEEFrame) response = service_handle(F_T_EE=trans_mat) rospy.loginfo("Set EE Frame Request Status: %s. \n\tDetails: %s" % ( "Success" if response.success else "Failed!", response.error)) return response.success except rospy.ServiceException as e: rospy.logwarn("Set EE Frame Request: Service call failed: %s" % e) return False def set_K_frame(self, frame): """ Set new K frame based on the transformation given by 'frame', which is the transformation matrix defining the new desired K frame with respect to the EE frame. :type frame: [float (16,)] / np.ndarray (4x4) :param frame: transformation matrix of new K frame wrt EE frame :rtype: bool :return: success status of service request """ frame = self._assert_frame_validity(frame) return self._request_setK_service(frame) def set_K_at_frame(self, frame_name, timeout=5.0): """ Set new K frame to the same frame as the link frame given by 'frame_name' Only 'panda_link7', 'panda_link8', 'panda_hand', 'panda_NE', and 'panda_EE' are valid frames for this operation. Other frames will not produce required results. Motion controllers should be stopped for switching. :type frame_name: str :param frame_name: desired tf frame name in the tf tree :rtype: [bool, str] :return: [success status of service request, error msg if any] """ return self.set_K_frame(self.get_link_tf(frame_name, timeout, parent="panda_EE")) def get_K_frame(self, as_mat=False): """ Get current K frame transformation matrix in EE frame :type as_mat: bool :param as_mat: if True, return np array, else as list :rtype: [float (16,)] / np.ndarray (4x4) :return: transformation matrix of K frame wrt EE frame """ return self._current_K_frame_transformation if not as_mat else np.asarray(self._current_K_frame_transformation).reshape(4, 4, order='F') def reset_K_frame(self): """ Reset K frame to default. (defined by DEFAULT_K_FRAME global variable defined above) By default this resets to align the stiffness frame to the end-effector frame. :rtype: bool :return: success status of service request """ return self.set_K_frame(frame=DEFAULT_TRANSFORMATIONS.K_FRAME) def K_frame_is_reset(self): assert self._current_K_frame_transformation is not None, "FrankaFramesInterface: Current K Frame is not known." return list(self._current_K_frame_transformation) == list(DEFAULT_TRANSFORMATIONS.K_FRAME) def _request_setK_service(self, trans_mat): rospy.wait_for_service( '/franka_ros_interface/franka_control/set_K_frame') try: service_handle = rospy.ServiceProxy( '/franka_ros_interface/franka_control/set_K_frame', SetKFrame) response = service_handle(EE_T_K=trans_mat) rospy.loginfo("Set K Frame Request Status: %s. \n\tDetails: %s" % ( "Success" if response.success else "Failed!", response.error)) return response.success except rospy.ServiceException as e: rospy.logwarn("Set K Frame Request: Service call failed: %s" % e) return False if __name__ == '__main__': rospy.init_node("test") ee_setter = FrankaFramesInterface() # ee_setter.set_EE_frame([1,0,0,0,0,1,0,0,0,0,1,0,0,0,0,1]) ================================================ FILE: franka_tools/src/franka_tools/joint_trajectory_action_client.py ================================================ # /*************************************************************************** # # @package: franka_tools # @metapackage: franka_ros_interface # @author: Saif Sidhik # # **************************************************************************/ # /*************************************************************************** # Copyright (c) 2019-2021, Saif Sidhik # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. # You may obtain a copy of the License at # http://www.apache.org/licenses/LICENSE-2.0 # Unless required by applicable law or agreed to in writing, software # distributed under the License is distributed on an "AS IS" BASIS, # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. # See the License for the specific language governing permissions and # limitations under the License. # **************************************************************************/ import rospy import sys import actionlib from copy import copy from control_msgs.msg import ( FollowJointTrajectoryAction, FollowJointTrajectoryGoal, ) from trajectory_msgs.msg import ( JointTrajectoryPoint, ) class JointTrajectoryActionClient(object): """ To use this class, the currently active controller for the franka robot should be the "joint_position_trajectory_controller". This can be set using instance of :py:class:`franka_tools.FrankaControllerManagerInterface`. """ def __init__(self, joint_names, controller_name = "position_joint_trajectory_controller"): self._joint_names = joint_names self._client = actionlib.SimpleActionClient("/%s/follow_joint_trajectory"%(controller_name), FollowJointTrajectoryAction, ) self._goal = FollowJointTrajectoryGoal() self._goal_time_tolerance = rospy.Time(0.1) self._goal.goal_time_tolerance = self._goal_time_tolerance server_up = self._client.wait_for_server(timeout=rospy.Duration(3.0)) if not server_up: rospy.logerr("Timed out waiting for Joint Trajectory" " Action Server to connect. Start the action server" " before running example.") rospy.signal_shutdown("Timed out waiting for Action Server") sys.exit(1) self.clear() def add_point(self, positions, time, velocities = None): """ Add a waypoint to the trajectory. :param positions: target joint positions :type positions: [float]*7 :param time: target time in seconds from the start of trajectory to reach the specified goal :type time: float :param velocities: goal velocities for joints (give atleast 0.0001) :type velocities: [float]*7 .. note:: Velocities should be greater than zero (done by default) for smooth motion. """ point = JointTrajectoryPoint() point.positions = copy(positions) if velocities is None: point.velocities = [0.0001 for n in positions] else: point.velocities = copy(velocities) point.time_from_start = rospy.Duration(time) self._goal.trajectory.points.append(point) def start(self): """ Execute previously defined trajectory. """ self._goal.trajectory.header.stamp = rospy.Time.now() self._client.send_goal(self._goal) def stop(self): """ Stop currently executing trajectory. """ self._client.cancel_goal() def wait(self, timeout=15.0): """ Wait for trajectory execution result. :param timeout: timeout before cancelling wait :type timeout: float """ self._client.wait_for_result(timeout=rospy.Duration(timeout)) def result(self): """ Get result from trajectory action server """ return self._client.get_result() def clear(self): """ Clear all waypoints from the current trajectory definition. """ self._goal = FollowJointTrajectoryGoal() self._goal.goal_time_tolerance = self._goal_time_tolerance self._goal.trajectory.joint_names = self._joint_names if __name__ == '__main__': pass