├── pyfri ├── tools │ ├── __init__.py │ ├── filters.py │ └── state_estimators.py ├── __init__.py └── src │ └── wrapper.cpp ├── .gitmodules ├── doc ├── img │ └── applications_lbr_server.png ├── pyfri.rst └── example_applications.rst ├── examples ├── robots │ ├── meshes │ │ ├── med14 │ │ │ ├── visual │ │ │ │ ├── link_0.stl │ │ │ │ ├── link_1.stl │ │ │ │ ├── link_2.stl │ │ │ │ ├── link_3.stl │ │ │ │ ├── link_4.stl │ │ │ │ ├── link_5.stl │ │ │ │ ├── link_6.stl │ │ │ │ └── link_7.stl │ │ │ └── collision │ │ │ │ ├── link_0.stl │ │ │ │ ├── link_1.stl │ │ │ │ ├── link_2.stl │ │ │ │ ├── link_3.stl │ │ │ │ ├── link_4.stl │ │ │ │ ├── link_5.stl │ │ │ │ ├── link_6.stl │ │ │ │ └── link_7.stl │ │ └── med7 │ │ │ ├── visual │ │ │ ├── link_0.stl │ │ │ ├── link_1.stl │ │ │ ├── link_2.stl │ │ │ ├── link_3.stl │ │ │ ├── link_4.stl │ │ │ ├── link_5.stl │ │ │ ├── link_6.stl │ │ │ └── link_7.stl │ │ │ └── collision │ │ │ ├── link_0.stl │ │ │ ├── link_1.stl │ │ │ ├── link_2.stl │ │ │ ├── link_3.stl │ │ │ ├── link_4.stl │ │ │ ├── link_5.stl │ │ │ ├── link_6.stl │ │ │ └── link_7.stl │ ├── materials.urdf.xacro │ ├── med14.urdf.xacro │ ├── med7.urdf.xacro │ ├── med14_description.urdf.xacro │ └── med7_description.urdf.xacro ├── robot.py ├── admittance.py ├── ik.py ├── hand_guide.py ├── LBRTorqueSineOverlay.py ├── LBRWrenchSineOverlay.py ├── LBRJointSineOverlay.py ├── joint_teleop.py └── task_teleop.py ├── .github ├── workflows │ ├── black.yaml │ └── build.yaml └── CONTRIBUTING.md ├── NOTICE ├── CITATION.cff ├── CMakeLists.txt ├── README.md ├── .gitignore ├── .clang-format ├── CODE_OF_CONDUCT.md ├── setup.py └── LICENSE /pyfri/tools/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /pyfri/__init__.py: -------------------------------------------------------------------------------- 1 | from _pyfri import * 2 | -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "pybind"] 2 | path = pybind 3 | url = git@github.com:pybind/pybind11.git 4 | -------------------------------------------------------------------------------- /doc/img/applications_lbr_server.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lbr-stack/pyfri/HEAD/doc/img/applications_lbr_server.png -------------------------------------------------------------------------------- /examples/robots/meshes/med14/visual/link_0.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lbr-stack/pyfri/HEAD/examples/robots/meshes/med14/visual/link_0.stl -------------------------------------------------------------------------------- /examples/robots/meshes/med14/visual/link_1.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lbr-stack/pyfri/HEAD/examples/robots/meshes/med14/visual/link_1.stl -------------------------------------------------------------------------------- /examples/robots/meshes/med14/visual/link_2.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lbr-stack/pyfri/HEAD/examples/robots/meshes/med14/visual/link_2.stl -------------------------------------------------------------------------------- /examples/robots/meshes/med14/visual/link_3.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lbr-stack/pyfri/HEAD/examples/robots/meshes/med14/visual/link_3.stl -------------------------------------------------------------------------------- /examples/robots/meshes/med14/visual/link_4.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lbr-stack/pyfri/HEAD/examples/robots/meshes/med14/visual/link_4.stl -------------------------------------------------------------------------------- /examples/robots/meshes/med14/visual/link_5.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lbr-stack/pyfri/HEAD/examples/robots/meshes/med14/visual/link_5.stl -------------------------------------------------------------------------------- /examples/robots/meshes/med14/visual/link_6.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lbr-stack/pyfri/HEAD/examples/robots/meshes/med14/visual/link_6.stl -------------------------------------------------------------------------------- /examples/robots/meshes/med14/visual/link_7.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lbr-stack/pyfri/HEAD/examples/robots/meshes/med14/visual/link_7.stl -------------------------------------------------------------------------------- /examples/robots/meshes/med7/visual/link_0.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lbr-stack/pyfri/HEAD/examples/robots/meshes/med7/visual/link_0.stl -------------------------------------------------------------------------------- /examples/robots/meshes/med7/visual/link_1.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lbr-stack/pyfri/HEAD/examples/robots/meshes/med7/visual/link_1.stl -------------------------------------------------------------------------------- /examples/robots/meshes/med7/visual/link_2.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lbr-stack/pyfri/HEAD/examples/robots/meshes/med7/visual/link_2.stl -------------------------------------------------------------------------------- /examples/robots/meshes/med7/visual/link_3.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lbr-stack/pyfri/HEAD/examples/robots/meshes/med7/visual/link_3.stl -------------------------------------------------------------------------------- /examples/robots/meshes/med7/visual/link_4.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lbr-stack/pyfri/HEAD/examples/robots/meshes/med7/visual/link_4.stl -------------------------------------------------------------------------------- /examples/robots/meshes/med7/visual/link_5.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lbr-stack/pyfri/HEAD/examples/robots/meshes/med7/visual/link_5.stl -------------------------------------------------------------------------------- /examples/robots/meshes/med7/visual/link_6.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lbr-stack/pyfri/HEAD/examples/robots/meshes/med7/visual/link_6.stl -------------------------------------------------------------------------------- /examples/robots/meshes/med7/visual/link_7.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lbr-stack/pyfri/HEAD/examples/robots/meshes/med7/visual/link_7.stl -------------------------------------------------------------------------------- /examples/robots/meshes/med7/collision/link_0.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lbr-stack/pyfri/HEAD/examples/robots/meshes/med7/collision/link_0.stl -------------------------------------------------------------------------------- /examples/robots/meshes/med7/collision/link_1.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lbr-stack/pyfri/HEAD/examples/robots/meshes/med7/collision/link_1.stl -------------------------------------------------------------------------------- /examples/robots/meshes/med7/collision/link_2.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lbr-stack/pyfri/HEAD/examples/robots/meshes/med7/collision/link_2.stl -------------------------------------------------------------------------------- /examples/robots/meshes/med7/collision/link_3.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lbr-stack/pyfri/HEAD/examples/robots/meshes/med7/collision/link_3.stl -------------------------------------------------------------------------------- /examples/robots/meshes/med7/collision/link_4.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lbr-stack/pyfri/HEAD/examples/robots/meshes/med7/collision/link_4.stl -------------------------------------------------------------------------------- /examples/robots/meshes/med7/collision/link_5.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lbr-stack/pyfri/HEAD/examples/robots/meshes/med7/collision/link_5.stl -------------------------------------------------------------------------------- /examples/robots/meshes/med7/collision/link_6.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lbr-stack/pyfri/HEAD/examples/robots/meshes/med7/collision/link_6.stl -------------------------------------------------------------------------------- /examples/robots/meshes/med7/collision/link_7.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lbr-stack/pyfri/HEAD/examples/robots/meshes/med7/collision/link_7.stl -------------------------------------------------------------------------------- /examples/robots/meshes/med14/collision/link_0.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lbr-stack/pyfri/HEAD/examples/robots/meshes/med14/collision/link_0.stl -------------------------------------------------------------------------------- /examples/robots/meshes/med14/collision/link_1.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lbr-stack/pyfri/HEAD/examples/robots/meshes/med14/collision/link_1.stl -------------------------------------------------------------------------------- /examples/robots/meshes/med14/collision/link_2.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lbr-stack/pyfri/HEAD/examples/robots/meshes/med14/collision/link_2.stl -------------------------------------------------------------------------------- /examples/robots/meshes/med14/collision/link_3.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lbr-stack/pyfri/HEAD/examples/robots/meshes/med14/collision/link_3.stl -------------------------------------------------------------------------------- /examples/robots/meshes/med14/collision/link_4.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lbr-stack/pyfri/HEAD/examples/robots/meshes/med14/collision/link_4.stl -------------------------------------------------------------------------------- /examples/robots/meshes/med14/collision/link_5.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lbr-stack/pyfri/HEAD/examples/robots/meshes/med14/collision/link_5.stl -------------------------------------------------------------------------------- /examples/robots/meshes/med14/collision/link_6.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lbr-stack/pyfri/HEAD/examples/robots/meshes/med14/collision/link_6.stl -------------------------------------------------------------------------------- /examples/robots/meshes/med14/collision/link_7.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lbr-stack/pyfri/HEAD/examples/robots/meshes/med14/collision/link_7.stl -------------------------------------------------------------------------------- /doc/pyfri.rst: -------------------------------------------------------------------------------- 1 | .. include:: ../README.md 2 | :parser: myst_parser.sphinx_ 3 | 4 | .. toctree:: 5 | :hidden: 6 | 7 | software_architecture 8 | example_applications 9 | -------------------------------------------------------------------------------- /.github/workflows/black.yaml: -------------------------------------------------------------------------------- 1 | name: Lint 2 | 3 | on: [push, pull_request] 4 | 5 | jobs: 6 | lint: 7 | runs-on: ubuntu-latest 8 | steps: 9 | - uses: actions/checkout@v3 10 | - uses: psf/black@stable 11 | -------------------------------------------------------------------------------- /examples/robot.py: -------------------------------------------------------------------------------- 1 | import optas 2 | 3 | 4 | def load_robot(lbr_med_num, time_derivs): 5 | xacro_file_name = f"robots/med{lbr_med_num}.urdf.xacro" 6 | robot = optas.RobotModel(xacro_filename=xacro_file_name, time_derivs=time_derivs) 7 | return robot 8 | -------------------------------------------------------------------------------- /NOTICE: -------------------------------------------------------------------------------- 1 | ===================================== 2 | Third party software 3 | ===================================== 4 | This software includes third party software. 5 | The license of all third party software is 6 | correctly included. 7 | 8 | ===================================== 9 | -------------------------------------------------------------------------------- /examples/robots/materials.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /examples/robots/med14.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /examples/robots/med7.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /.github/workflows/build.yaml: -------------------------------------------------------------------------------- 1 | name: Build 2 | 3 | on: 4 | pull_request: 5 | branches: 6 | - main 7 | workflow_dispatch: 8 | schedule: 9 | - cron: "0 0 * * *" 10 | 11 | jobs: 12 | build: 13 | runs-on: ${{ matrix.os }} 14 | strategy: 15 | matrix: 16 | os: [ubuntu-latest, ubuntu-22.04] 17 | fri_version: [1.11, 1.14, 1.15, 1.16, 2.5, 2.7] 18 | steps: 19 | - uses: actions/checkout@v4 20 | with: 21 | submodules: recursive 22 | - name: Set up Python 23 | uses: actions/setup-python@v5 24 | with: 25 | python-version: "3.x" 26 | - name: Test installation 27 | run: export FRI_CLIENT_VERSION=${{ matrix.fri_version }} && pip3 install --verbose . 28 | -------------------------------------------------------------------------------- /CITATION.cff: -------------------------------------------------------------------------------- 1 | cff-version: 1.2.0 2 | message: "If you enjoyed using this repository for your work, we would really appreciate ❤️ if you could cite it, as it helps us to continue offering support." 3 | authors: 4 | - family-names: Huber 5 | given-names: Martin 6 | orcid: https://orcid.org/0000-0003-4603-6773 7 | - family-names: Mower 8 | given-names: Christopher E. 9 | orcid: https://orcid.org/0000-0002-3929-9391 10 | - family-names: Ourselin 11 | given-names: Sebastien 12 | orcid: https://orcid.org/0000-0002-5694-5340 13 | - family-names: Vercauteren 14 | given-names: Tom 15 | orcid: https://orcid.org/0000-0003-1794-0456 16 | - family-names: Bergeles 17 | given-names: Christos 18 | orcid: https://orcid.org/0000-0002-9152-3194 19 | 20 | 21 | title: "LBR-Stack: ROS 2 and Python Integration of KUKA FRI for Med and IIWA Robots" 22 | version: 1.4.2 23 | doi: 10.48550/arXiv.2311.12709 24 | date-released: 2023-12-29 25 | -------------------------------------------------------------------------------- /.github/CONTRIBUTING.md: -------------------------------------------------------------------------------- 1 | The [LBR-Stack](https://github.com/lbr-stack/) thrives to build a friendly community for `KUKA` users to foster knowledge exchange! :) 2 | 3 | We **encourage** everyone to open issues for questions / problems / features. For additional information, we would like to provide some links below. 4 | 5 | ### Knowledge Sources 6 | - [Open issues](https://github.com/lbr-stack/pyfri/issues) 7 | - [Closed issues](https://github.com/lbr-stack/pyfri/issues?q=is%3Aissue+is%3Aclosed) 8 | 9 | ## Contributing 10 | ### pyfri 11 | Contributions are vital to this project. If you want to contribute a feature / fix proceed as follows 12 | 13 | 1. Open an issue: 14 | - Explain the issue and your solution 15 | - Request a new branch: `dev-feature`, e.g. `dev-my-new-demo` 16 | 2. Fork this repository 17 | 3. Create a [pull request](https://github.com/lbr-stack/pyfri/pulls) against `dev-` 18 | 19 | ### New FRI version 20 | Refer [README](https://github.com/lbr-stack/fri#contributing) for instructions. 21 | -------------------------------------------------------------------------------- /pyfri/tools/filters.py: -------------------------------------------------------------------------------- 1 | import abc 2 | import numpy as np 3 | from collections import deque 4 | 5 | 6 | class StateFilter(abc.ABC): 7 | def __init__(self, window_size): 8 | self._window_size = window_size 9 | self.reset() 10 | 11 | def reset(self): 12 | self._window = deque([], maxlen=self._window_size) 13 | 14 | def append(self, x): 15 | self._window.append(x.tolist()) 16 | 17 | @abc.abstractmethod 18 | def filter(self, x): 19 | pass 20 | 21 | 22 | class ExponentialStateFilter(StateFilter): 23 | def __init__(self, smooth=0.02): 24 | super().__init__(1) 25 | self._smooth = smooth 26 | 27 | def filter(self, x): 28 | if len(self._window) == 0: 29 | self.append(x) 30 | xp = np.array(self._window[0]) 31 | xf = self._smooth * x + (1.0 - self._smooth) * xp 32 | self.append(xf) 33 | return xf 34 | 35 | 36 | class MovingAverageFilter(StateFilter): 37 | def filter(self, x): 38 | self.append(x) 39 | return np.mean(self._window, axis=0) 40 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | ######################################################################### 2 | ## NOTE: Do not invoke this cmake yourself, use pip install . (see the ## 3 | ## README.md file). ## 4 | ######################################################################### 5 | 6 | cmake_minimum_required(VERSION 3.16.3) 7 | 8 | project(_pyfri) 9 | 10 | include(FetchContent) 11 | 12 | set(CMAKE_POSITION_INDEPENDENT_CODE ON) 13 | set(FRI_BUILD_EXAMPLES OFF) 14 | 15 | # set the FRI version 16 | set(FRI_CLIENT_VERSION_MAJOR 1 CACHE STRING "The FRI client major version." FORCE) 17 | set(FRI_CLIENT_VERSION_MINOR 15 CACHE STRING "The FRI client minor version." FORCE) 18 | 19 | # fetch the fri depending on the version 20 | FetchContent_Declare( 21 | FRI 22 | GIT_REPOSITORY https://github.com/lbr-stack/fri 23 | GIT_TAG fri-${FRI_CLIENT_VERSION_MAJOR}.${FRI_CLIENT_VERSION_MINOR} 24 | ) 25 | 26 | FetchContent_MakeAvailable(FRI) 27 | 28 | add_subdirectory(pybind) 29 | 30 | pybind11_add_module(_pyfri ${CMAKE_CURRENT_SOURCE_DIR}/pyfri/src/wrapper.cpp) 31 | 32 | target_include_directories( 33 | _pyfri 34 | PRIVATE 35 | ${CMAKE_CURRENT_SOURCE_DIR}/pyfri/src 36 | ) 37 | 38 | target_link_libraries(_pyfri PRIVATE FRIClient) 39 | -------------------------------------------------------------------------------- /examples/admittance.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import optas 3 | from robot import load_robot 4 | 5 | 6 | class AdmittanceController: 7 | def __init__(self, lbr_med_num): 8 | # Setup robot 9 | self.ee_link = "lbr_link_ee" 10 | self.robot = load_robot(lbr_med_num, [1]) 11 | self.name = self.robot.get_name() 12 | 13 | # Setup builder 14 | T = 1 15 | builder = optas.OptimizationBuilder(T, robots=self.robot, derivs_align=True) 16 | 17 | # Set parameters 18 | qc = builder.add_parameter("qc", self.robot.ndof) # current joint position 19 | vg = builder.add_parameter( 20 | "vg", 6 21 | ) # task space velocity goal: [vx, vy, vz, wx, wy, wz] 22 | dt = builder.add_parameter("dt") # time step 23 | 24 | # Get model states 25 | dq = builder.get_model_state(self.name, t=0, time_deriv=1) 26 | 27 | # Cost: end-effector goal velocity 28 | J = self.robot.get_global_link_geometric_jacobian(self.ee_link, qc) 29 | v = J @ dq 30 | builder.add_cost_term("ee_vel_goal", 50.0 * optas.sumsqr(v - vg)) 31 | 32 | # Constraint: joint limits 33 | q = qc + dt * dq 34 | builder.add_bound_inequality_constraint( 35 | "joint_limits", 36 | self.robot.lower_actuated_joint_limits, 37 | q, 38 | self.robot.upper_actuated_joint_limits, 39 | ) 40 | 41 | # Cost: minimize joint velocity 42 | builder.add_cost_term("min_joint_vel", optas.sumsqr(dq)) 43 | 44 | # Setup solver 45 | opt = builder.build() 46 | solver_options = {"printLevel": "none"} 47 | self.solver = optas.CasADiSolver(opt).setup("qpoases", solver_options) 48 | self.solution = None 49 | self.gain = np.array([0.05, 0.05, 0.05, 0.3, 0.3, 0.3]) 50 | self.vlim = np.concatenate(([0.2, 0.2, 0.2], np.deg2rad([40] * 3))) 51 | 52 | def __call__(self, qc, wr, dt): 53 | # Admittance control: map force -> velocity 54 | vg = self.gain * wr 55 | 56 | # Clip velocity (safety) 57 | vg = np.clip(vg, -self.vlim, self.vlim) 58 | 59 | # Setup solver 60 | if self.solution is not None: 61 | self.solver.reset_initial_seed(self.solution) 62 | 63 | self.solver.reset_parameters({"qc": qc, "vg": vg, "dt": dt}) 64 | 65 | # Solve problem and retrieve solution 66 | self.solution = self.solver.solve() 67 | dqg = self.solution[f"{self.name}/dq"].toarray().flatten() 68 | qg = qc + dt * dqg 69 | 70 | return qg 71 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # pyfri 2 | 3 | [![Build](https://github.com/lbr-stack/pyfri/actions/workflows/build.yaml/badge.svg)](https://github.com/lbr-stack/pyfri/actions/workflows/build.yaml) 4 | [![License](https://img.shields.io/github/license/lbr-stack/pyfri)](https://github.com/lbr-stack/pyfri/tree/main?tab=Apache-2.0-1-ov-file#readme) 5 | [![JOSS](https://joss.theoj.org/papers/c43c82bed833c02503dd47f2637192ef/status.svg)](https://joss.theoj.org/papers/c43c82bed833c02503dd47f2637192ef) 6 | [![Code Style: Black](https://img.shields.io/badge/code%20style-black-000000.svg)](https://github.com/psf/black) 7 | 8 | KUKA Fast Robot Interface Python SDK. 9 | The code in this repository, provides Python bindings for the FRI Client SDK C++ through [pybind11](https://github.com/pybind/pybind11). 10 | The interface has been designed to be as similar as possible to the documentation provided by KUKA. 11 | 12 | There is one difference users of the Python bindings should be aware. 13 | When instantiating the client application, in C++ this is performed as follows. 14 | 15 | ```cpp 16 | // ..setup client.. 17 | 18 | // create new udp connection 19 | UdpConnection connection; 20 | 21 | 22 | // pass connection and client to a new FRI client application 23 | ClientApplication app(connection, client); 24 | ``` 25 | 26 | In Python, the equivalent code is as follows. 27 | 28 | ```python 29 | import pyfri as fri 30 | 31 | # ..setup client.. 32 | 33 | app = fri.ClientApplication(client) 34 | ``` 35 | 36 | Since UDP is the only supported connection type and the connection object is not actually required by the user after declaring the variable, the `UdpConnection` object is created internally to the `fri.ClientApplication` class object. 37 | 38 | See the [examples](examples/). 39 | 40 | ## Quickstart 41 | 42 | 1. Clone repository (make sure you include `--recursive`): 43 | ```shell 44 | git clone --recursive https://github.com/lbr-stack/pyfri.git 45 | ``` 46 | 2. Change directory: 47 | ```shell 48 | cd pyfri 49 | ``` 50 | 3. Install: 51 | ```shell 52 | export FRI_CLIENT_VERSION=1.15 53 | pip3 install . 54 | ``` 55 | 56 | > [!NOTE] 57 | > FRI client is fetched from [fri](https://github.com/lbr-stack/fri) and must be available as branch, refer [README](https://github.com/lbr-stack/fri?tab=readme-ov-file#contributing). 58 | 59 | 4. Setup the hardware, see [Hardware Setup](https://lbr-stack.readthedocs.io/en/latest/lbr_fri_ros2_stack/lbr_fri_ros2_stack/doc/hardware_setup.html). 60 | 61 | 5. Checkout the [Example Applications](https://lbr-stack.readthedocs.io/en/latest/pyfri/doc/example_applications.html#example-applications). 62 | 63 | ## Citation 64 | If you enjoyed using this repository for your work, we would really appreciate ❤️ if you could leave a ⭐ and / or cite it, as it helps us to continue offering support. 65 | 66 | ``` 67 | @misc{huber2023lbrstack, 68 | title={LBR-Stack: ROS 2 and Python Integration of KUKA FRI for Med and IIWA Robots}, 69 | author={Martin Huber and Christopher E. Mower and Sebastien Ourselin and Tom Vercauteren and Christos Bergeles}, 70 | year={2023}, 71 | eprint={2311.12709}, 72 | archivePrefix={arXiv}, 73 | primaryClass={cs.RO} 74 | } 75 | ``` 76 | -------------------------------------------------------------------------------- /examples/ik.py: -------------------------------------------------------------------------------- 1 | import optas 2 | from robot import load_robot 3 | 4 | 5 | class IK: 6 | """ 7 | 8 | This class solves the following problem 9 | 10 | q0*, qf*, dq* = arg min w1 || J(qc) dq - vg ||^2 + w2 || dq ||^2 11 | q0, qf, dq 12 | 13 | subject to 14 | q0 = qc (initial configuration) 15 | q- <= q0, qf <= q+ (joint limits) 16 | q0 + dt*dq = qf (dynamics) 17 | 18 | 19 | Decision variables 20 | q0: initial joint state 21 | qf: final joint state 22 | dq: joint velocity 23 | 24 | Parameters 25 | qc: current joint state 26 | dt: time step 27 | w1, w2: cost term weights 28 | vg: task space velocity goal (linear and angular) 29 | q-, q+: lower, upper joint limits 30 | 31 | Function 32 | J(..): geometric Jacobian 33 | 34 | """ 35 | 36 | def __init__(self, lbr_med_num): 37 | # Setup robot 38 | ee_link = "lbr_link_ee" 39 | self.robot = load_robot(lbr_med_num, [0, 1]) 40 | self.name = self.robot.get_name() 41 | 42 | # Setup builder 43 | T = 2 44 | builder = optas.OptimizationBuilder(T, robots=self.robot) 45 | 46 | # Set parameters 47 | qc = builder.add_parameter("qc", self.robot.ndof) # current joint position 48 | vg = builder.add_parameter( 49 | "vg", 6 50 | ) # task space velocity goal: [vx, vy, vz, wx, wy, wz] 51 | dt = builder.add_parameter("dt") # time step 52 | 53 | # Get model states 54 | qf = builder.get_model_state(self.name, t=1, time_deriv=0) 55 | dq = builder.get_model_state(self.name, t=0, time_deriv=1) 56 | 57 | # Cost: end-effector goal velocity 58 | J = self.robot.get_global_link_geometric_jacobian(ee_link, qc) 59 | builder.add_cost_term("ee_vel_goal", 50.0 * optas.sumsqr(J @ dq - vg)) 60 | 61 | # Constraint: initial configuration 62 | builder.initial_configuration(self.name, qc) 63 | 64 | # Constraint: dynamics 65 | builder.integrate_model_states(self.name, 1, dt) 66 | 67 | # Constraint: joint limits 68 | builder.enforce_model_limits(self.name, safe_frac=0.95) 69 | 70 | # Cost: minimize joint velocity 71 | builder.add_cost_term("min_joint_vel", 0.01 * optas.sumsqr(dq)) 72 | 73 | # Setup solver 74 | opt = builder.build() 75 | self.solver = optas.CasADiSolver(opt).setup("qpoases") 76 | self.solution = None 77 | 78 | def __call__(self, qc, vg, dt): 79 | # Reset initial seed with previous solution 80 | if self.solution is not None: 81 | self.solver.reset_initial_seed(self.solution) 82 | else: 83 | self.solver.reset_initial_seed({f"{self.name}/q": optas.horzcat(qc, qc)}) 84 | 85 | # Reset parameters 86 | self.solver.reset_parameters({"qc": qc, "vg": vg, "dt": dt}) 87 | 88 | # Solve problem 89 | self.solution = self.solver.solve() 90 | 91 | # Return solution or current position if solver failed 92 | if self.solver.did_solve(): 93 | return self.solution[f"{self.name}/q"][:, 1].toarray().flatten() 94 | else: 95 | print("[WARN] solver failed!") 96 | return qc 97 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # Byte-compiled / optimized / DLL files 2 | __pycache__/ 3 | *.py[cod] 4 | *$py.class 5 | 6 | # C extensions 7 | *.so 8 | 9 | # Distribution / packaging 10 | .Python 11 | build/ 12 | develop-eggs/ 13 | dist/ 14 | downloads/ 15 | eggs/ 16 | .eggs/ 17 | lib/ 18 | lib64/ 19 | parts/ 20 | sdist/ 21 | var/ 22 | wheels/ 23 | share/python-wheels/ 24 | *.egg-info/ 25 | .installed.cfg 26 | *.egg 27 | MANIFEST 28 | 29 | # PyInstaller 30 | # Usually these files are written by a python script from a template 31 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 32 | *.manifest 33 | *.spec 34 | 35 | # Installer logs 36 | pip-log.txt 37 | pip-delete-this-directory.txt 38 | 39 | # Unit test / coverage reports 40 | htmlcov/ 41 | .tox/ 42 | .nox/ 43 | .coverage 44 | .coverage.* 45 | .cache 46 | nosetests.xml 47 | coverage.xml 48 | *.cover 49 | *.py,cover 50 | .hypothesis/ 51 | .pytest_cache/ 52 | cover/ 53 | 54 | # Translations 55 | *.mo 56 | *.pot 57 | 58 | # Django stuff: 59 | *.log 60 | local_settings.py 61 | db.sqlite3 62 | db.sqlite3-journal 63 | 64 | # Flask stuff: 65 | instance/ 66 | .webassets-cache 67 | 68 | # Scrapy stuff: 69 | .scrapy 70 | 71 | # Sphinx documentation 72 | docs/_build/ 73 | 74 | # PyBuilder 75 | .pybuilder/ 76 | target/ 77 | 78 | # Jupyter Notebook 79 | .ipynb_checkpoints 80 | 81 | # IPython 82 | profile_default/ 83 | ipython_config.py 84 | 85 | # pyenv 86 | # For a library or package, you might want to ignore these files since the code is 87 | # intended to run in multiple environments; otherwise, check them in: 88 | # .python-version 89 | 90 | # pipenv 91 | # According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control. 92 | # However, in case of collaboration, if having platform-specific dependencies or dependencies 93 | # having no cross-platform support, pipenv may install dependencies that don't work, or not 94 | # install all needed dependencies. 95 | #Pipfile.lock 96 | 97 | # poetry 98 | # Similar to Pipfile.lock, it is generally recommended to include poetry.lock in version control. 99 | # This is especially recommended for binary packages to ensure reproducibility, and is more 100 | # commonly ignored for libraries. 101 | # https://python-poetry.org/docs/basic-usage/#commit-your-poetrylock-file-to-version-control 102 | #poetry.lock 103 | 104 | # pdm 105 | # Similar to Pipfile.lock, it is generally recommended to include pdm.lock in version control. 106 | #pdm.lock 107 | # pdm stores project-wide configurations in .pdm.toml, but it is recommended to not include it 108 | # in version control. 109 | # https://pdm.fming.dev/#use-with-ide 110 | .pdm.toml 111 | 112 | # PEP 582; used by e.g. github.com/David-OConnor/pyflow and github.com/pdm-project/pdm 113 | __pypackages__/ 114 | 115 | # Celery stuff 116 | celerybeat-schedule 117 | celerybeat.pid 118 | 119 | # SageMath parsed files 120 | *.sage.py 121 | 122 | # Environments 123 | .env 124 | .venv 125 | env/ 126 | venv/ 127 | ENV/ 128 | env.bak/ 129 | venv.bak/ 130 | 131 | # Spyder project settings 132 | .spyderproject 133 | .spyproject 134 | 135 | # Rope project settings 136 | .ropeproject 137 | 138 | # mkdocs documentation 139 | /site 140 | 141 | # mypy 142 | .mypy_cache/ 143 | .dmypy.json 144 | dmypy.json 145 | 146 | # Pyre type checker 147 | .pyre/ 148 | 149 | # pytype static type analyzer 150 | .pytype/ 151 | 152 | # Cython debug symbols 153 | cython_debug/ 154 | 155 | # PyCharm 156 | # JetBrains specific template is maintained in a separate JetBrains.gitignore that can 157 | # be found at https://github.com/github/gitignore/blob/main/Global/JetBrains.gitignore 158 | # and can be added to the global gitignore or merged into this file. For a more nuclear 159 | # option (not recommended) you can uncomment the following to ignore the entire idea folder. 160 | #.idea/ 161 | -------------------------------------------------------------------------------- /examples/hand_guide.py: -------------------------------------------------------------------------------- 1 | import argparse 2 | import sys 3 | 4 | import numpy as np 5 | from admittance import AdmittanceController 6 | 7 | import pyfri as fri 8 | from pyfri.tools.filters import ExponentialStateFilter 9 | from pyfri.tools.state_estimators import ( 10 | FRIExternalTorqueEstimator, 11 | JointStateEstimator, 12 | WrenchEstimatorTaskOffset, 13 | ) 14 | 15 | if fri.FRI_CLIENT_VERSION_MAJOR == 1: 16 | POSITION = fri.EClientCommandMode.POSITION 17 | elif fri.FRI_CLIENT_VERSION_MAJOR == 2: 18 | POSITION = fri.EClientCommandMode.JOINT_POSITION 19 | 20 | 21 | class HandGuideClient(fri.LBRClient): 22 | def __init__(self, lbr_ver): 23 | super().__init__() 24 | self.controller = AdmittanceController(lbr_ver) 25 | self.joint_state_estimator = JointStateEstimator(self) 26 | self.external_torque_estimator = FRIExternalTorqueEstimator(self) 27 | self.wrench_estimator = WrenchEstimatorTaskOffset( 28 | self, 29 | self.joint_state_estimator, 30 | self.external_torque_estimator, 31 | self.controller.robot, 32 | self.controller.ee_link, 33 | ) 34 | self.wrench_filter = ExponentialStateFilter() 35 | 36 | def command_position(self): 37 | self.robotCommand().setJointPosition(self.q.astype(np.float32)) 38 | 39 | def monitor(self): 40 | pass 41 | 42 | def onStateChange(self, old_state, new_state): 43 | print(f"State changed from {old_state} to {new_state}") 44 | 45 | def waitForCommand(self): 46 | if self.robotState().getClientCommandMode() != POSITION: 47 | print( 48 | f"[ERROR] hand guide example requires {POSITION.name} client command mode" 49 | ) 50 | raise SystemExit 51 | 52 | self.q = self.robotState().getIpoJointPosition() 53 | self.command_position() 54 | 55 | def command(self): 56 | if not self.wrench_estimator.ready(): 57 | self.wrench_estimator.update() 58 | self.robotCommand().setJointPosition(self.q.astype(np.float32)) 59 | return 60 | 61 | # Get robot state 62 | wr = self.wrench_estimator.get_wrench() 63 | dt = self.robotState().getSampleTime() 64 | 65 | # Filter wrench 66 | wf = self.wrench_filter.filter(wr) 67 | 68 | # Compute goal using admittance controller 69 | self.q = self.controller(self.q, wf, dt) 70 | 71 | # Command robot 72 | self.command_position() 73 | 74 | 75 | def args_factory(): 76 | parser = argparse.ArgumentParser(description="LRBJointSineOverlay example.") 77 | parser.add_argument( 78 | "--hostname", 79 | dest="hostname", 80 | default=None, 81 | help="The hostname used to communicate with the KUKA Sunrise Controller.", 82 | ) 83 | parser.add_argument( 84 | "--port", 85 | dest="port", 86 | type=int, 87 | default=30200, 88 | help="The port number used to communicate with the KUKA Sunrise Controller.", 89 | ) 90 | parser.add_argument( 91 | "--lbr-ver", 92 | dest="lbr_ver", 93 | type=int, 94 | choices=[7, 14], 95 | required=True, 96 | help="The KUKA LBR Med version number.", 97 | ) 98 | 99 | return parser.parse_args() 100 | 101 | 102 | def main(): 103 | print("Running FRI Version:", fri.FRI_CLIENT_VERSION) 104 | 105 | args = args_factory() 106 | client = HandGuideClient(args.lbr_ver) 107 | app = fri.ClientApplication(client) 108 | success = app.connect(args.port, args.hostname) 109 | 110 | if not success: 111 | print("Connection to KUKA Sunrise controller failed.") 112 | return 1 113 | 114 | try: 115 | while success: 116 | success = app.step() 117 | 118 | if client.robotState().getSessionState() == fri.ESessionState.IDLE: 119 | break 120 | 121 | except KeyboardInterrupt: 122 | pass 123 | 124 | except SystemExit: 125 | pass 126 | 127 | finally: 128 | app.disconnect() 129 | print("Goodbye") 130 | 131 | return 0 132 | 133 | 134 | if __name__ == "__main__": 135 | sys.exit(main()) 136 | -------------------------------------------------------------------------------- /examples/LBRTorqueSineOverlay.py: -------------------------------------------------------------------------------- 1 | import argparse 2 | import math 3 | import sys 4 | 5 | import numpy as np 6 | 7 | import pyfri as fri 8 | 9 | 10 | class LBRTorqueSineOverlayClient(fri.LBRClient): 11 | def __init__(self, joint_mask, freq_hz, torque_amplitude): 12 | super().__init__() 13 | self.joint_mask = joint_mask 14 | self.freq_hz = freq_hz 15 | self.torque_ampl = torque_amplitude 16 | self.phi = 0.0 17 | self.step_width = 0.0 18 | self.torques = np.zeros(fri.LBRState.NUMBER_OF_JOINTS, dtype=np.float32) 19 | 20 | def monitor(self): 21 | pass 22 | 23 | def onStateChange(self, old_state, new_state): 24 | print(f"State changed from {old_state} to {new_state}") 25 | 26 | if new_state == fri.ESessionState.MONITORING_READY: 27 | self.torques = np.zeros(fri.LBRState.NUMBER_OF_JOINTS, dtype=np.float32) 28 | self.phi = 0.0 29 | self.step_width = ( 30 | 2 * math.pi * self.freq_hz * self.robotState().getSampleTime() 31 | ) 32 | 33 | def waitForCommand(self): 34 | self.robotCommand().setJointPosition(self.robotState().getIpoJointPosition()) 35 | 36 | if self.robotState().getClientCommandMode() == fri.EClientCommandMode.TORQUE: 37 | self.robotCommand().setTorque(self.torques) 38 | 39 | def command(self): 40 | self.robotCommand().setJointPosition(self.robotState().getIpoJointPosition()) 41 | 42 | if self.robotState().getClientCommandMode() == fri.EClientCommandMode.TORQUE: 43 | offset = self.torque_ampl * math.sin(self.phi) 44 | self.phi += self.step_width 45 | 46 | if self.phi >= 2 * math.pi: 47 | self.phi -= 2 * math.pi 48 | 49 | self.torques[self.joint_mask] = offset 50 | 51 | self.robotCommand().setTorque(self.torques) 52 | 53 | 54 | def args_factory(): 55 | def cvt_joint_mask(value): 56 | int_value = int(value) 57 | if 0 <= int_value < 7: 58 | return int_value 59 | else: 60 | raise argparse.ArgumentTypeError(f"{value} is not in the range [0, 7).") 61 | 62 | parser = argparse.ArgumentParser(description="LRBJointSineOverlay example.") 63 | parser.add_argument( 64 | "--hostname", 65 | dest="hostname", 66 | default=None, 67 | help="The hostname used to communicate with the KUKA Sunrise Controller.", 68 | ) 69 | parser.add_argument( 70 | "--port", 71 | dest="port", 72 | type=int, 73 | default=30200, 74 | help="The port number used to communicate with the KUKA Sunrise Controller.", 75 | ) 76 | parser.add_argument( 77 | "--joint-mask", 78 | dest="joint_mask", 79 | type=cvt_joint_mask, 80 | default=3, 81 | help="The joint to move.", 82 | ) 83 | parser.add_argument( 84 | "--freq-hz", 85 | dest="freq_hz", 86 | type=float, 87 | default=0.25, 88 | help="The frequency of the sine wave.", 89 | ) 90 | parser.add_argument( 91 | "--torque-amplitude", 92 | dest="torque_amplitude", 93 | type=float, 94 | default=15.0, 95 | help="Applitude of the sine wave.", 96 | ) 97 | 98 | return parser.parse_args() 99 | 100 | 101 | def main(): 102 | print("Running FRI Version:", fri.FRI_CLIENT_VERSION) 103 | 104 | args = args_factory() 105 | client = LBRTorqueSineOverlayClient( 106 | args.joint_mask, args.freq_hz, args.torque_amplitude 107 | ) 108 | app = fri.ClientApplication(client) 109 | success = app.connect(args.port, args.hostname) 110 | 111 | if not success: 112 | print("Connection to KUKA Sunrise controller failed.") 113 | return 1 114 | 115 | try: 116 | while success: 117 | success = app.step() 118 | 119 | if client.robotState().getSessionState() == fri.ESessionState.IDLE: 120 | break 121 | 122 | except KeyboardInterrupt: 123 | pass 124 | 125 | finally: 126 | app.disconnect() 127 | print("Goodbye") 128 | 129 | return 0 130 | 131 | 132 | if __name__ == "__main__": 133 | sys.exit(main()) 134 | -------------------------------------------------------------------------------- /examples/LBRWrenchSineOverlay.py: -------------------------------------------------------------------------------- 1 | import argparse 2 | import math 3 | import sys 4 | 5 | import numpy as np 6 | 7 | import pyfri as fri 8 | 9 | 10 | class LBRWrenchSineOverlayClient(fri.LBRClient): 11 | def __init__(self, frequencyX, frequencyY, amplitudeX, amplitudeY): 12 | super().__init__() 13 | self.frequencyX = frequencyX 14 | self.frequencyY = frequencyY 15 | self.amplitudeX = amplitudeX 16 | self.amplitudeY = amplitudeY 17 | self.stepWidthX = 0.0 18 | self.stepWidthY = 0.0 19 | self.phiX = 0.0 20 | self.phiY = 0.0 21 | self.wrench = np.zeros(6, dtype=np.float32) 22 | 23 | def monitor(self): 24 | pass 25 | 26 | def onStateChange(self, old_state, new_state): 27 | if new_state == fri.ESessionState.MONITORING_READY: 28 | self.phiX = 0.0 29 | self.phiY = 0.0 30 | self.stepWidthX = ( 31 | 2.0 * math.pi * self.frequencyX * self.robotState().getSampleTime() 32 | ) 33 | self.stepWidthY = ( 34 | 2.0 * math.pi * self.frequencyY * self.robotState().getSampleTime() 35 | ) 36 | 37 | def waitForCommand(self): 38 | self.robotCommand().setJointPosition(self.robotState().getIpoJointPosition()) 39 | if self.robotState().getClientCommandMode() == fri.EClientCommandMode.WRENCH: 40 | self.robotCommand().setWrench(self.wrench) 41 | 42 | def command(self): 43 | self.robotCommand().setJointPosition(self.robotState().getIpoJointPosition()) 44 | if self.robotState().getClientCommandMode() == fri.EClientCommandMode.WRENCH: 45 | self.wrench[0] = self.amplitudeX * math.sin(self.phiX) 46 | self.wrench[1] = self.amplitudeY * math.sin(self.phiY) 47 | 48 | self.phiX += self.stepWidthX 49 | self.phiY += self.stepWidthY 50 | 51 | if self.phiX >= 2.0 * math.pi: 52 | self.phiX -= 2.0 * math.pi 53 | 54 | if self.phiY >= 2.0 * math.pi: 55 | self.phiY -= 2.0 * math.pi 56 | 57 | self.robotCommand().setWrench(self.wrench) 58 | 59 | 60 | def args_factory(): 61 | parser = argparse.ArgumentParser(description="LRBJointSineOverlay example.") 62 | parser.add_argument( 63 | "--hostname", 64 | dest="hostname", 65 | default=None, 66 | help="The hostname used to communicate with the KUKA Sunrise Controller.", 67 | ) 68 | parser.add_argument( 69 | "--port", 70 | dest="port", 71 | type=int, 72 | default=30200, 73 | help="The port number used to communicate with the KUKA Sunrise Controller.", 74 | ) 75 | parser.add_argument( 76 | "--frequencyx", 77 | dest="frequencyX", 78 | type=float, 79 | default=0.25, 80 | help="The frequency of sine wave in x-axis.", 81 | ) 82 | parser.add_argument( 83 | "--frequencyy", 84 | dest="frequencyY", 85 | type=float, 86 | default=0.25, 87 | help="The frequency of sine wave in y-axis.", 88 | ) 89 | parser.add_argument( 90 | "--amplitudex", 91 | dest="amplitudeX", 92 | type=float, 93 | default=5.0, 94 | help="The amplitude of sine wave in x-axis.", 95 | ) 96 | parser.add_argument( 97 | "--amplitudey", 98 | dest="amplitudeY", 99 | type=float, 100 | default=5.0, 101 | help="The amplitude of sine wave in y-axis.", 102 | ) 103 | 104 | return parser.parse_args() 105 | 106 | 107 | def main(): 108 | print("Running FRI Version:", fri.FRI_CLIENT_VERSION) 109 | 110 | args = args_factory() 111 | print(args) 112 | client = LBRWrenchSineOverlayClient( 113 | args.frequencyX, args.frequencyY, args.amplitudeX, args.amplitudeY 114 | ) 115 | app = fri.ClientApplication(client) 116 | success = app.connect(args.port, args.hostname) 117 | 118 | if not success: 119 | print("Connection to KUKA Sunrise controller failed.") 120 | return 1 121 | 122 | try: 123 | while success: 124 | success = app.step() 125 | 126 | if client.robotState().getSessionState() == fri.ESessionState.IDLE: 127 | break 128 | 129 | except KeyboardInterrupt: 130 | pass 131 | 132 | finally: 133 | app.disconnect() 134 | 135 | return 0 136 | 137 | 138 | if __name__ == "__main__": 139 | sys.exit(main()) 140 | -------------------------------------------------------------------------------- /.clang-format: -------------------------------------------------------------------------------- 1 | --- 2 | Language: Cpp 3 | # BasedOnStyle: LLVM 4 | AccessModifierOffset: -2 5 | AlignAfterOpenBracket: Align 6 | AlignConsecutiveMacros: false 7 | AlignConsecutiveAssignments: false 8 | AlignConsecutiveDeclarations: false 9 | AlignEscapedNewlines: Right 10 | AlignOperands: true 11 | AlignTrailingComments: true 12 | AllowAllArgumentsOnNextLine: true 13 | AllowAllConstructorInitializersOnNextLine: true 14 | AllowAllParametersOfDeclarationOnNextLine: true 15 | AllowShortBlocksOnASingleLine: Never 16 | AllowShortCaseLabelsOnASingleLine: false 17 | AllowShortFunctionsOnASingleLine: All 18 | AllowShortLambdasOnASingleLine: All 19 | AllowShortIfStatementsOnASingleLine: Never 20 | AllowShortLoopsOnASingleLine: false 21 | AlwaysBreakAfterDefinitionReturnType: None 22 | AlwaysBreakAfterReturnType: None 23 | AlwaysBreakBeforeMultilineStrings: false 24 | AlwaysBreakTemplateDeclarations: MultiLine 25 | BinPackArguments: true 26 | BinPackParameters: true 27 | BraceWrapping: 28 | AfterCaseLabel: false 29 | AfterClass: false 30 | AfterControlStatement: false 31 | AfterEnum: false 32 | AfterFunction: false 33 | AfterNamespace: false 34 | AfterObjCDeclaration: false 35 | AfterStruct: false 36 | AfterUnion: false 37 | AfterExternBlock: false 38 | BeforeCatch: false 39 | BeforeElse: false 40 | IndentBraces: false 41 | SplitEmptyFunction: true 42 | SplitEmptyRecord: true 43 | SplitEmptyNamespace: true 44 | BreakBeforeBinaryOperators: None 45 | BreakBeforeBraces: Attach 46 | BreakBeforeInheritanceComma: false 47 | BreakInheritanceList: BeforeColon 48 | BreakBeforeTernaryOperators: true 49 | BreakConstructorInitializersBeforeComma: false 50 | BreakConstructorInitializers: BeforeColon 51 | BreakAfterJavaFieldAnnotations: false 52 | BreakStringLiterals: true 53 | ColumnLimit: 80 54 | CommentPragmas: '^ IWYU pragma:' 55 | CompactNamespaces: false 56 | ConstructorInitializerAllOnOneLineOrOnePerLine: false 57 | ConstructorInitializerIndentWidth: 4 58 | ContinuationIndentWidth: 4 59 | Cpp11BracedListStyle: true 60 | DeriveLineEnding: true 61 | DerivePointerAlignment: false 62 | DisableFormat: false 63 | ExperimentalAutoDetectBinPacking: false 64 | FixNamespaceComments: true 65 | ForEachMacros: 66 | - foreach 67 | - Q_FOREACH 68 | - BOOST_FOREACH 69 | IncludeBlocks: Preserve 70 | IncludeCategories: 71 | - Regex: '^"(llvm|llvm-c|clang|clang-c)/' 72 | Priority: 2 73 | SortPriority: 0 74 | - Regex: '^(<|"(gtest|gmock|isl|json)/)' 75 | Priority: 3 76 | SortPriority: 0 77 | - Regex: '.*' 78 | Priority: 1 79 | SortPriority: 0 80 | IncludeIsMainRegex: '(Test)?$' 81 | IncludeIsMainSourceRegex: '' 82 | IndentCaseLabels: false 83 | IndentGotoLabels: true 84 | IndentPPDirectives: None 85 | IndentWidth: 2 86 | IndentWrappedFunctionNames: false 87 | JavaScriptQuotes: Leave 88 | JavaScriptWrapImports: true 89 | KeepEmptyLinesAtTheStartOfBlocks: true 90 | MacroBlockBegin: '' 91 | MacroBlockEnd: '' 92 | MaxEmptyLinesToKeep: 1 93 | NamespaceIndentation: None 94 | ObjCBinPackProtocolList: Auto 95 | ObjCBlockIndentWidth: 2 96 | ObjCSpaceAfterProperty: false 97 | ObjCSpaceBeforeProtocolList: true 98 | PenaltyBreakAssignment: 2 99 | PenaltyBreakBeforeFirstCallParameter: 19 100 | PenaltyBreakComment: 300 101 | PenaltyBreakFirstLessLess: 120 102 | PenaltyBreakString: 1000 103 | PenaltyBreakTemplateDeclaration: 10 104 | PenaltyExcessCharacter: 1000000 105 | PenaltyReturnTypeOnItsOwnLine: 60 106 | PointerAlignment: Right 107 | ReflowComments: true 108 | SortIncludes: true 109 | SortUsingDeclarations: true 110 | SpaceAfterCStyleCast: false 111 | SpaceAfterLogicalNot: false 112 | SpaceAfterTemplateKeyword: true 113 | SpaceBeforeAssignmentOperators: true 114 | SpaceBeforeCpp11BracedList: false 115 | SpaceBeforeCtorInitializerColon: true 116 | SpaceBeforeInheritanceColon: true 117 | SpaceBeforeParens: ControlStatements 118 | SpaceBeforeRangeBasedForLoopColon: true 119 | SpaceInEmptyBlock: false 120 | SpaceInEmptyParentheses: false 121 | SpacesBeforeTrailingComments: 1 122 | SpacesInAngles: false 123 | SpacesInConditionalStatement: false 124 | SpacesInContainerLiterals: true 125 | SpacesInCStyleCastParentheses: false 126 | SpacesInParentheses: false 127 | SpacesInSquareBrackets: false 128 | SpaceBeforeSquareBrackets: false 129 | Standard: Latest 130 | StatementMacros: 131 | - Q_UNUSED 132 | - QT_REQUIRE_VERSION 133 | TabWidth: 8 134 | UseCRLF: false 135 | UseTab: Never 136 | ... 137 | 138 | -------------------------------------------------------------------------------- /examples/LBRJointSineOverlay.py: -------------------------------------------------------------------------------- 1 | import argparse 2 | import math 3 | import sys 4 | 5 | import matplotlib.pyplot as plt 6 | import numpy as np 7 | import pandas as pd 8 | 9 | import pyfri as fri 10 | 11 | 12 | class LBRJointSineOverlayClient(fri.LBRClient): 13 | def __init__(self, joint_mask, freq_hz, ampl_rad, filter_coeff): 14 | super().__init__() 15 | self.joint_mask = joint_mask 16 | self.freq_hz = freq_hz 17 | self.ampl_rad = ampl_rad 18 | self.filter_coeff = filter_coeff 19 | self.offset = 0.0 20 | self.phi = 0.0 21 | self.step_width = 0.0 22 | 23 | def monitor(self): 24 | pass 25 | 26 | def onStateChange(self, old_state, new_state): 27 | print(f"Changed state from {old_state} to {new_state}") 28 | if new_state == fri.ESessionState.MONITORING_READY: 29 | self.offset = 0.0 30 | self.phi = 0.0 31 | self.step_width = ( 32 | 2 * math.pi * self.freq_hz * self.robotState().getSampleTime() 33 | ) 34 | 35 | def waitForCommand(self): 36 | self.robotCommand().setJointPosition( 37 | self.robotState().getIpoJointPosition().astype(np.float32) 38 | ) 39 | 40 | def command(self): 41 | new_offset = self.ampl_rad * math.sin(self.phi) 42 | self.offset = (self.offset * self.filter_coeff) + ( 43 | new_offset * (1.0 - self.filter_coeff) 44 | ) 45 | self.phi += self.step_width 46 | if self.phi >= (2 * math.pi): 47 | self.phi -= 2 * math.pi 48 | joint_pos = self.robotState().getIpoJointPosition() 49 | joint_pos[self.joint_mask] += self.offset 50 | self.robotCommand().setJointPosition(joint_pos.astype(np.float32)) 51 | 52 | 53 | def args_factory(): 54 | def cvt_joint_mask(value): 55 | int_value = int(value) 56 | if 0 <= int_value < 7: 57 | return int_value 58 | else: 59 | raise argparse.ArgumentTypeError(f"{value} is not in the range [0, 7).") 60 | 61 | parser = argparse.ArgumentParser(description="LRBJointSineOverlay example.") 62 | parser.add_argument( 63 | "--hostname", 64 | dest="hostname", 65 | default=None, 66 | help="The hostname used to communicate with the KUKA Sunrise Controller.", 67 | ) 68 | parser.add_argument( 69 | "--port", 70 | dest="port", 71 | type=int, 72 | default=30200, 73 | help="The port number used to communicate with the KUKA Sunrise Controller.", 74 | ) 75 | parser.add_argument( 76 | "--joint-mask", 77 | dest="joint_mask", 78 | type=cvt_joint_mask, 79 | default=3, 80 | help="The joint to move.", 81 | ) 82 | parser.add_argument( 83 | "--freq-hz", 84 | dest="freq_hz", 85 | type=float, 86 | default=0.25, 87 | help="The frequency of the sine wave.", 88 | ) 89 | parser.add_argument( 90 | "--ampl-rad", 91 | dest="ampl_rad", 92 | type=float, 93 | default=0.04, 94 | help="Applitude of the sine wave.", 95 | ) 96 | parser.add_argument( 97 | "--filter-coeff", 98 | dest="filter_coeff", 99 | type=float, 100 | default=0.99, 101 | help="Exponential smoothing coeficient.", 102 | ) 103 | parser.add_argument( 104 | "--save-data", 105 | dest="save_data", 106 | action="store_true", 107 | default=False, 108 | help="Set this flag to save the data.", 109 | ) 110 | 111 | return parser.parse_args() 112 | 113 | 114 | def main(): 115 | print("Running FRI Version:", fri.FRI_CLIENT_VERSION) 116 | 117 | args = args_factory() 118 | client = LBRJointSineOverlayClient( 119 | args.joint_mask, args.freq_hz, args.ampl_rad, args.filter_coeff 120 | ) 121 | app = fri.ClientApplication(client) 122 | if args.save_data: 123 | app.collect_data("lbr_joint_sine_overlay.csv") 124 | success = app.connect(args.port, args.hostname) 125 | 126 | if not success: 127 | print("Connection to KUKA Sunrise controller failed.") 128 | return 1 129 | 130 | try: 131 | while success: 132 | success = app.step() 133 | 134 | if client.robotState().getSessionState() == fri.ESessionState.IDLE: 135 | break 136 | 137 | except KeyboardInterrupt: 138 | pass 139 | 140 | finally: 141 | app.disconnect() 142 | if args.save_data: 143 | df = pd.read_csv("lbr_joint_sine_overlay.csv") 144 | 145 | fig, ax = plt.subplots(4, 1, sharex=True) 146 | 147 | dim2name = { 148 | "mp": "Measured Position", 149 | "ip": "Ipo Position", 150 | "mt": "Measured Torque", 151 | "et": "External Torque", 152 | } 153 | 154 | for i, dim in enumerate(["mp", "ip", "mt", "et"]): 155 | df.plot(x="time", y=[dim + str(i + 1) for i in range(7)], ax=ax[i]) 156 | ax[i].set_ylabel(dim2name[dim]) 157 | ax[-1].set_xlabel("Time (s)") 158 | 159 | plt.show() 160 | 161 | return 0 162 | 163 | 164 | if __name__ == "__main__": 165 | sys.exit(main()) 166 | -------------------------------------------------------------------------------- /CODE_OF_CONDUCT.md: -------------------------------------------------------------------------------- 1 | # Contributor Covenant Code of Conduct 2 | 3 | ## Our Pledge 4 | 5 | We as members, contributors, and leaders pledge to make participation in our 6 | community a harassment-free experience for everyone, regardless of age, body 7 | size, visible or invisible disability, ethnicity, sex characteristics, gender 8 | identity and expression, level of experience, education, socio-economic status, 9 | nationality, personal appearance, race, religion, or sexual identity 10 | and orientation. 11 | 12 | We pledge to act and interact in ways that contribute to an open, welcoming, 13 | diverse, inclusive, and healthy community. 14 | 15 | ## Our Standards 16 | 17 | Examples of behavior that contributes to a positive environment for our 18 | community include: 19 | 20 | * Demonstrating empathy and kindness toward other people 21 | * Being respectful of differing opinions, viewpoints, and experiences 22 | * Giving and gracefully accepting constructive feedback 23 | * Accepting responsibility and apologizing to those affected by our mistakes, 24 | and learning from the experience 25 | * Focusing on what is best not just for us as individuals, but for the 26 | overall community 27 | 28 | Examples of unacceptable behavior include: 29 | 30 | * The use of sexualized language or imagery, and sexual attention or 31 | advances of any kind 32 | * Trolling, insulting or derogatory comments, and personal or political attacks 33 | * Public or private harassment 34 | * Publishing others' private information, such as a physical or email 35 | address, without their explicit permission 36 | * Other conduct which could reasonably be considered inappropriate in a 37 | professional setting 38 | 39 | ## Enforcement Responsibilities 40 | 41 | Community leaders are responsible for clarifying and enforcing our standards of 42 | acceptable behavior and will take appropriate and fair corrective action in 43 | response to any behavior that they deem inappropriate, threatening, offensive, 44 | or harmful. 45 | 46 | Community leaders have the right and responsibility to remove, edit, or reject 47 | comments, commits, code, wiki edits, issues, and other contributions that are 48 | not aligned to this Code of Conduct, and will communicate reasons for moderation 49 | decisions when appropriate. 50 | 51 | ## Scope 52 | 53 | This Code of Conduct applies within all community spaces, and also applies when 54 | an individual is officially representing the community in public spaces. 55 | Examples of representing our community include using an official e-mail address, 56 | posting via an official social media account, or acting as an appointed 57 | representative at an online or offline event. 58 | 59 | ## Enforcement 60 | 61 | Instances of abusive, harassing, or otherwise unacceptable behavior may be 62 | reported to the community leaders responsible for enforcement at 63 | https://github.com/lbr-stack/pyfri/issues. 64 | All complaints will be reviewed and investigated promptly and fairly. 65 | 66 | All community leaders are obligated to respect the privacy and security of the 67 | reporter of any incident. 68 | 69 | ## Enforcement Guidelines 70 | 71 | Community leaders will follow these Community Impact Guidelines in determining 72 | the consequences for any action they deem in violation of this Code of Conduct: 73 | 74 | ### 1. Correction 75 | 76 | **Community Impact**: Use of inappropriate language or other behavior deemed 77 | unprofessional or unwelcome in the community. 78 | 79 | **Consequence**: A private, written warning from community leaders, providing 80 | clarity around the nature of the violation and an explanation of why the 81 | behavior was inappropriate. A public apology may be requested. 82 | 83 | ### 2. Warning 84 | 85 | **Community Impact**: A violation through a single incident or series 86 | of actions. 87 | 88 | **Consequence**: A warning with consequences for continued behavior. No 89 | interaction with the people involved, including unsolicited interaction with 90 | those enforcing the Code of Conduct, for a specified period of time. This 91 | includes avoiding interactions in community spaces as well as external channels 92 | like social media. Violating these terms may lead to a temporary or 93 | permanent ban. 94 | 95 | ### 3. Temporary Ban 96 | 97 | **Community Impact**: A serious violation of community standards, including 98 | sustained inappropriate behavior. 99 | 100 | **Consequence**: A temporary ban from any sort of interaction or public 101 | communication with the community for a specified period of time. No public or 102 | private interaction with the people involved, including unsolicited interaction 103 | with those enforcing the Code of Conduct, is allowed during this period. 104 | Violating these terms may lead to a permanent ban. 105 | 106 | ### 4. Permanent Ban 107 | 108 | **Community Impact**: Demonstrating a pattern of violation of community 109 | standards, including sustained inappropriate behavior, harassment of an 110 | individual, or aggression toward or disparagement of classes of individuals. 111 | 112 | **Consequence**: A permanent ban from any sort of public interaction within 113 | the community. 114 | 115 | ## Attribution 116 | 117 | This Code of Conduct is adapted from the [Contributor Covenant][homepage], 118 | version 2.0, available at 119 | https://www.contributor-covenant.org/version/2/0/code_of_conduct.html. 120 | 121 | Community Impact Guidelines were inspired by [Mozilla's code of conduct 122 | enforcement ladder](https://github.com/mozilla/diversity). 123 | 124 | [homepage]: https://www.contributor-covenant.org 125 | 126 | For answers to common questions about this code of conduct, see the FAQ at 127 | https://www.contributor-covenant.org/faq. Translations are available at 128 | https://www.contributor-covenant.org/translations. 129 | -------------------------------------------------------------------------------- /examples/joint_teleop.py: -------------------------------------------------------------------------------- 1 | import argparse 2 | import sys 3 | 4 | # FRI Client: https://github.com/cmower/FRI-Client-SDK_Python 5 | import pyfri as fri 6 | 7 | # PyGame: https://www.pygame.org/news 8 | import pygame 9 | 10 | pygame.init() 11 | 12 | # NumPy: https://numpy.org/ 13 | import numpy as np 14 | 15 | 16 | class Keyboard: 17 | def __init__(self): 18 | pygame.display.set_mode((300, 300)) 19 | 20 | self.max_joint_velocity = np.deg2rad(5) 21 | 22 | self.joint_index = None 23 | self.joint_velocity = 0.0 24 | 25 | self.key_to_dir_map = { 26 | pygame.K_LEFT: 1.0, 27 | pygame.K_RIGHT: -1.0, 28 | } 29 | 30 | self.key_joint_map = [ 31 | pygame.K_1, 32 | pygame.K_2, 33 | pygame.K_3, 34 | pygame.K_4, 35 | pygame.K_5, 36 | pygame.K_6, 37 | pygame.K_7, 38 | ] 39 | 40 | def __call__(self): 41 | for event in pygame.event.get(): 42 | if event.type == pygame.QUIT: 43 | # User closed pygame window -> shutdown 44 | pygame.quit() 45 | raise SystemExit 46 | 47 | if event.type == pygame.KEYDOWN: 48 | # Keydown event 49 | 50 | if event.key == pygame.K_ESCAPE: 51 | pygame.quit() 52 | raise SystemExit 53 | 54 | if event.key in self.key_joint_map: 55 | joint_index = self.key_joint_map.index(event.key) 56 | 57 | if self.joint_index is None: 58 | self.joint_index = joint_index 59 | print(f"Turned ON joint {self.joint_index+1}") 60 | else: 61 | if joint_index == self.joint_index: 62 | print(f"Turned OFF joint {self.joint_index+1}") 63 | self.joint_index = None 64 | 65 | elif event.key in self.key_to_dir_map: 66 | self.joint_velocity += self.key_to_dir_map[event.key] 67 | 68 | elif event.type == pygame.KEYUP: 69 | # Keyup event 70 | 71 | if event.key in self.key_to_dir_map: 72 | self.joint_velocity -= self.key_to_dir_map[event.key] 73 | 74 | return self.joint_index, self.max_joint_velocity * self.joint_velocity 75 | 76 | 77 | class TeleopClient(fri.LBRClient): 78 | def __init__(self, keyboard): 79 | super().__init__() 80 | self.keyboard = keyboard 81 | self.torques = np.zeros(fri.LBRState.NUMBER_OF_JOINTS) 82 | 83 | def monitor(self): 84 | pass 85 | 86 | def onStateChange(self, old_state, new_state): 87 | print(f"State changed from {old_state} to {new_state}") 88 | 89 | if new_state == fri.ESessionState.MONITORING_READY: 90 | self.q = None 91 | self.torques = np.zeros(fri.LBRState.NUMBER_OF_JOINTS) 92 | 93 | print("\n") 94 | print("-----------------------------------------------------------") 95 | print("-- Control robot joints using LEFT/RIGHT direction keys. --") 96 | print("-- Press keys 1, ..., 7 to enable a specific joint. --") 97 | print("-- The PyGame window must be in focus. --") 98 | print( 99 | "-----------------------------------------------------------", 100 | end="\n\n\n", 101 | ) 102 | 103 | def waitForCommand(self): 104 | self.q = self.robotState().getIpoJointPosition() 105 | self.robotCommand().setJointPosition(self.q.astype(np.float32)) 106 | 107 | if self.robotState().getClientCommandMode() == fri.EClientCommandMode.TORQUE: 108 | self.robotCommand().setTorque(self.torques.astype(np.float32)) 109 | 110 | def command(self): 111 | joint_index, vgoal = self.keyboard() 112 | 113 | if isinstance(joint_index, int): 114 | self.q[joint_index] += self.robotState().getSampleTime() * vgoal 115 | 116 | self.robotCommand().setJointPosition(self.q.astype(np.float32)) 117 | 118 | if self.robotState().getClientCommandMode() == fri.EClientCommandMode.TORQUE: 119 | self.robotCommand().setTorque(self.torques.astype(np.float32)) 120 | 121 | 122 | def args_factory(): 123 | parser = argparse.ArgumentParser(description="LRBJointSineOverlay example.") 124 | parser.add_argument( 125 | "--hostname", 126 | dest="hostname", 127 | default=None, 128 | help="The hostname used to communicate with the KUKA Sunrise Controller.", 129 | ) 130 | parser.add_argument( 131 | "--port", 132 | dest="port", 133 | type=int, 134 | default=30200, 135 | help="The port number used to communicate with the KUKA Sunrise Controller.", 136 | ) 137 | 138 | return parser.parse_args() 139 | 140 | 141 | def main(): 142 | print("Running FRI Version:", fri.FRI_CLIENT_VERSION) 143 | 144 | args = args_factory() 145 | keyboard = Keyboard() 146 | client = TeleopClient(keyboard) 147 | app = fri.ClientApplication(client) 148 | success = app.connect(args.port, args.hostname) 149 | 150 | if not success: 151 | print("Connection to KUKA Sunrise controller failed.") 152 | return 1 153 | 154 | print("Connection to KUKA Sunrise controller established.") 155 | 156 | try: 157 | while success: 158 | success = app.step() 159 | 160 | if client.robotState().getSessionState() == fri.ESessionState.IDLE: 161 | break 162 | 163 | except KeyboardInterrupt: 164 | pass 165 | 166 | except SystemExit: 167 | pass 168 | 169 | finally: 170 | app.disconnect() 171 | print("Goodbye") 172 | 173 | return 0 174 | 175 | 176 | if __name__ == "__main__": 177 | sys.exit(main()) 178 | -------------------------------------------------------------------------------- /examples/task_teleop.py: -------------------------------------------------------------------------------- 1 | import argparse 2 | import sys 3 | from collections import OrderedDict 4 | 5 | # PyGame: https://www.pygame.org/news 6 | import pygame 7 | 8 | # FRI Client: https://github.com/cmower/FRI-Client-SDK_Python 9 | import pyfri as fri 10 | 11 | pygame.init() 12 | 13 | # NumPy: https://numpy.org/ 14 | import numpy as np 15 | 16 | np.set_printoptions(precision=5, suppress=True, linewidth=1000) 17 | 18 | # Local scripts 19 | from ik import IK 20 | 21 | 22 | def print_instructions(): 23 | print("\n") 24 | print("-" * 65) 25 | print("-- Control robot joints using LEFT/RIGHT direction keys. --") 26 | print("-- Press keys x, y, z, r, p, a to enable a specific task axis. --") 27 | print("-- The PyGame window must be in focus. --") 28 | print("-" * 65, end="\n\n\n") 29 | 30 | 31 | class Keyboard: 32 | def __init__(self): 33 | pygame.display.set_mode((300, 300)) 34 | 35 | self.max_task_velocity = 0.04 36 | 37 | self.task_index = None 38 | self.task_velocity = 0.0 39 | 40 | self.key_to_dir_map = { 41 | pygame.K_LEFT: 1.0, 42 | pygame.K_RIGHT: -1.0, 43 | } 44 | 45 | self.key_task_map = OrderedDict() 46 | self.key_task_map[pygame.K_x] = "x" 47 | self.key_task_map[pygame.K_y] = "y" 48 | self.key_task_map[pygame.K_z] = "z" 49 | self.key_task_map[pygame.K_r] = "rx" 50 | self.key_task_map[pygame.K_p] = "ry" 51 | self.key_task_map[pygame.K_a] = "rz" 52 | 53 | def __call__(self): 54 | for event in pygame.event.get(): 55 | if event.type == pygame.QUIT: 56 | # User closed pygame window -> shutdown 57 | pygame.quit() 58 | raise SystemExit 59 | 60 | if event.type == pygame.KEYDOWN: 61 | # Keydown event 62 | 63 | if event.key == pygame.K_ESCAPE: 64 | pygame.quit() 65 | raise SystemExit 66 | 67 | if event.key in self.key_task_map: 68 | task_index = list(self.key_task_map.keys()).index(event.key) 69 | task_label = self.key_task_map[event.key] 70 | 71 | if self.task_index is None: 72 | self.task_index = task_index 73 | print(f"Turned ON task {task_label}") 74 | else: 75 | if task_index == self.task_index: 76 | print(f"Turned OFF task {task_label}") 77 | self.task_index = None 78 | 79 | elif event.key in self.key_to_dir_map: 80 | self.task_velocity += self.key_to_dir_map[event.key] 81 | 82 | elif event.type == pygame.KEYUP: 83 | # Keyup event 84 | 85 | if event.key in self.key_to_dir_map: 86 | self.task_velocity -= self.key_to_dir_map[event.key] 87 | 88 | return self.task_index, self.max_task_velocity * self.task_velocity 89 | 90 | 91 | class TeleopClient(fri.LBRClient): 92 | def __init__(self, ik, keyboard): 93 | super().__init__() 94 | self.ik = ik 95 | self.keyboard = keyboard 96 | self.torques = np.zeros(fri.LBRState.NUMBER_OF_JOINTS) 97 | 98 | def monitor(self): 99 | pass 100 | 101 | def onStateChange(self, old_state, new_state): 102 | print(f"State changed from {old_state} to {new_state}") 103 | 104 | if new_state == fri.ESessionState.MONITORING_READY: 105 | self.q = None 106 | self.torques = np.zeros(fri.LBRState.NUMBER_OF_JOINTS) 107 | 108 | print_instructions() 109 | 110 | def waitForCommand(self): 111 | self.q = self.robotState().getIpoJointPosition() 112 | self.robotCommand().setJointPosition(self.q.astype(np.float32)) 113 | 114 | if self.robotState().getClientCommandMode() == fri.EClientCommandMode.TORQUE: 115 | self.robotCommand().setTorque(self.torques.astype(np.float32)) 116 | 117 | def command(self): 118 | task_index, vgoal = self.keyboard() 119 | 120 | if isinstance(task_index, int): 121 | vg = np.zeros(len(self.keyboard.key_task_map)) 122 | vg[task_index] = vgoal 123 | 124 | self.q = self.ik(self.q, vg, self.robotState().getSampleTime()) 125 | 126 | self.robotCommand().setJointPosition(self.q.astype(np.float32)) 127 | 128 | if self.robotState().getClientCommandMode() == fri.EClientCommandMode.TORQUE: 129 | self.robotCommand().setTorque(self.torques.astype(np.float32)) 130 | 131 | 132 | def args_factory(): 133 | parser = argparse.ArgumentParser(description="LRBJointSineOverlay example.") 134 | parser.add_argument( 135 | "--hostname", 136 | dest="hostname", 137 | default=None, 138 | help="The hostname used to communicate with the KUKA Sunrise Controller.", 139 | ) 140 | parser.add_argument( 141 | "--port", 142 | dest="port", 143 | type=int, 144 | default=30200, 145 | help="The port number used to communicate with the KUKA Sunrise Controller.", 146 | ) 147 | parser.add_argument( 148 | "--lbr-ver", 149 | dest="lbr_ver", 150 | type=int, 151 | choices=[7, 14], 152 | required=True, 153 | help="The KUKA LBR Med version number.", 154 | ) 155 | 156 | return parser.parse_args() 157 | 158 | 159 | def main(): 160 | print("Running FRI Version:", fri.FRI_CLIENT_VERSION) 161 | 162 | args = args_factory() 163 | ik = IK(args.lbr_ver) 164 | keyboard = Keyboard() 165 | client = TeleopClient(ik, keyboard) 166 | app = fri.ClientApplication(client) 167 | success = app.connect(args.port, args.hostname) 168 | 169 | if not success: 170 | print("Connection to KUKA Sunrise controller failed.") 171 | return 1 172 | 173 | print("Connection to KUKA Sunrise controller established.") 174 | 175 | try: 176 | while success: 177 | success = app.step() 178 | 179 | if client.robotState().getSessionState() == fri.ESessionState.IDLE: 180 | break 181 | 182 | except KeyboardInterrupt: 183 | pass 184 | 185 | except SystemExit: 186 | pass 187 | 188 | finally: 189 | app.disconnect() 190 | print("Goodbye") 191 | 192 | return 0 193 | 194 | 195 | if __name__ == "__main__": 196 | sys.exit(main()) 197 | -------------------------------------------------------------------------------- /setup.py: -------------------------------------------------------------------------------- 1 | import os 2 | import re 3 | import subprocess 4 | import sys 5 | from pathlib import Path 6 | 7 | from setuptools import Extension, find_packages, setup 8 | from setuptools.command.build_ext import build_ext 9 | 10 | 11 | # Read the configuration settings 12 | class UserInputRequired(Exception): 13 | def __init__(self, msg): 14 | super().__init__(msg) 15 | 16 | 17 | FRI_CLIENT_VERSION = os.environ.get("FRI_CLIENT_VERSION") 18 | if FRI_CLIENT_VERSION is None: 19 | raise UserInputRequired( 20 | "Please set the environment variable FRI_CLIENT_VERSION to the version of the FRI Client SDK you are using." 21 | ) 22 | 23 | # Convert distutils Windows platform specifiers to CMake -A arguments 24 | PLAT_TO_CMAKE = { 25 | "win32": "Win32", 26 | "win-amd64": "x64", 27 | "win-arm32": "ARM", 28 | "win-arm64": "ARM64", 29 | } 30 | 31 | 32 | # A CMakeExtension needs a sourcedir instead of a file list. 33 | # The name must be the _single_ output extension from the CMake build. 34 | # If you need multiple extensions, see scikit-build. 35 | class CMakeExtension(Extension): 36 | def __init__(self, name: str, sourcedir: str = "") -> None: 37 | super().__init__(name, sources=[]) 38 | self.sourcedir = os.fspath(Path(sourcedir).resolve()) 39 | 40 | 41 | class CMakeBuild(build_ext): 42 | def build_extension(self, ext: CMakeExtension) -> None: 43 | # Must be in this form due to bug in .resolve() only fixed in Python 3.10+ 44 | ext_fullpath = Path.cwd() / self.get_ext_fullpath(ext.name) 45 | extdir = ext_fullpath.parent.resolve() 46 | 47 | # Using this requires trailing slash for auto-detection & inclusion of 48 | # auxiliary "native" libs 49 | debug = int(os.environ.get("DEBUG", 0)) if self.debug is None else self.debug 50 | cfg = "Debug" if debug else "Release" 51 | 52 | # CMake lets you override the generator - we need to check this. 53 | # Can be set with Conda-Build, for example. 54 | cmake_generator = os.environ.get("CMAKE_GENERATOR", "") 55 | 56 | # Set Python_EXECUTABLE instead if you use PYBIND11_FINDPYTHON 57 | # EXAMPLE_VERSION_INFO shows you how to pass a value into the C++ code 58 | # from Python. 59 | cmake_args = [ 60 | f"-DCMAKE_LIBRARY_OUTPUT_DIRECTORY={extdir}{os.sep}", 61 | f"-DPYTHON_EXECUTABLE={sys.executable}", 62 | f"-DCMAKE_BUILD_TYPE={cfg}", # not used on MSVC, but no harm 63 | ] 64 | build_args = [] 65 | # Adding CMake arguments set as environment variable 66 | # (needed e.g. to build for ARM OSx on conda-forge) 67 | if "CMAKE_ARGS" in os.environ: 68 | cmake_args += [item for item in os.environ["CMAKE_ARGS"].split(" ") if item] 69 | 70 | # In this example, we pass in the version to C++. You might not need to. 71 | # cmake_args += [f"-DEXAMPLE_VERSION_INFO={self.distribution.get_version()}"] 72 | 73 | if self.compiler.compiler_type != "msvc": 74 | # Using Ninja-build since it a) is available as a wheel and b) 75 | # multithreads automatically. MSVC would require all variables be 76 | # exported for Ninja to pick it up, which is a little tricky to do. 77 | # Users can override the generator with CMAKE_GENERATOR in CMake 78 | # 3.15+. 79 | if not cmake_generator or cmake_generator == "Ninja": 80 | try: 81 | import ninja 82 | 83 | ninja_executable_path = Path(ninja.BIN_DIR) / "ninja" 84 | cmake_args += [ 85 | "-GNinja", 86 | f"-DCMAKE_MAKE_PROGRAM:FILEPATH={ninja_executable_path}", 87 | ] 88 | except ImportError: 89 | pass 90 | 91 | else: 92 | # Single config generators are handled "normally" 93 | single_config = any(x in cmake_generator for x in {"NMake", "Ninja"}) 94 | 95 | # CMake allows an arch-in-generator style for backward compatibility 96 | contains_arch = any(x in cmake_generator for x in {"ARM", "Win64"}) 97 | 98 | # Specify the arch if using MSVC generator, but only if it doesn't 99 | # contain a backward-compatibility arch spec already in the 100 | # generator name. 101 | if not single_config and not contains_arch: 102 | cmake_args += ["-A", PLAT_TO_CMAKE[self.plat_name]] 103 | 104 | # Multi-config generators have a different way to specify configs 105 | if not single_config: 106 | cmake_args += [ 107 | f"-DCMAKE_LIBRARY_OUTPUT_DIRECTORY_{cfg.upper()}={extdir}" 108 | ] 109 | build_args += ["--config", cfg] 110 | 111 | if sys.platform.startswith("darwin"): 112 | # Cross-compile support for macOS - respect ARCHFLAGS if set 113 | archs = re.findall(r"-arch (\S+)", os.environ.get("ARCHFLAGS", "")) 114 | if archs: 115 | cmake_args += ["-DCMAKE_OSX_ARCHITECTURES={}".format(";".join(archs))] 116 | 117 | # Set CMAKE_BUILD_PARALLEL_LEVEL to control the parallel build level 118 | # across all generators. 119 | if "CMAKE_BUILD_PARALLEL_LEVEL" not in os.environ: 120 | # self.parallel is a Python 3 only way to set parallel jobs by hand 121 | # using -j in the build_ext call, not supported by pip or PyPA-build. 122 | if hasattr(self, "parallel") and self.parallel: 123 | # CMake 3.12+ only. 124 | build_args += [f"-j{self.parallel}"] 125 | 126 | # Set the FRI version number 127 | fri_ver_major = FRI_CLIENT_VERSION.split(".")[0] 128 | fri_ver_minor = FRI_CLIENT_VERSION.split(".")[1] 129 | cmake_args += [f"-DFRI_CLIENT_VERSION_MAJOR={fri_ver_major}"] 130 | cmake_args += [f"-DFRI_CLIENT_VERSION_MINOR={fri_ver_minor}"] 131 | 132 | build_temp = Path(self.build_temp) / ext.name 133 | if not build_temp.exists(): 134 | build_temp.mkdir(parents=True) 135 | 136 | subprocess.run( 137 | ["cmake", ext.sourcedir, *cmake_args], cwd=build_temp, check=True 138 | ) 139 | subprocess.run( 140 | ["cmake", "--build", ".", *build_args], cwd=build_temp, check=True 141 | ) 142 | 143 | 144 | setup( 145 | name="pyfri", 146 | version="1.2.1", 147 | author="Christopher E. Mower, Martin Huber", 148 | author_email="christopher.mower@kcl.ac.uk, m.huber_1994@hotmail.de", 149 | description="Python bindings for the FRI Client SDK library.", 150 | long_description="", 151 | packages=find_packages(), 152 | ext_modules=[CMakeExtension("_pyfri")], 153 | install_requires=["numpy", "pygame", "pyoptas", "pandas", "matplotlib"], 154 | cmdclass={"build_ext": CMakeBuild}, 155 | python_requires=">=3.8", 156 | ) 157 | -------------------------------------------------------------------------------- /doc/example_applications.rst: -------------------------------------------------------------------------------- 1 | Usage 2 | ----- 3 | .. contents:: Table of Contents 4 | :depth: 3 5 | :local: 6 | :backlinks: none 7 | 8 | Data Types 9 | ~~~~~~~~~~ 10 | 11 | You can pass NumPy arrays to the "set" methods (e.g. ``setJointPosition``) in order to command the robot. 12 | However, you **must** ensure the format of the array is correct. 13 | 14 | Arrays that have a ``dtype`` of ``np.float32`` are the only ones that can be accepted. 15 | See how the commands are set in the the examples. 16 | 17 | Collecting Data from the Robot 18 | ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ 19 | 20 | We provide additional functionality to the LBR client application class that enables data collection to a text file. 21 | In Python, simply add the following 22 | 23 | .. code-block:: python 24 | 25 | app = fri.ClientApplication(client) 26 | app.collect_data(file_name) 27 | 28 | The string ``file_name`` should contain the file name for the data file. 29 | We use the comma-separated values format for the data file, and so the file name should end with the extension ``.csv`` - *note*, if this isn't the case then ``.csv`` is automatically appended to the given file name. 30 | 31 | The columns in the recorded csv file are as follows: 32 | 33 | - ``index``: the index of the recorded data, starts at 0, and is incremented by 1 at each call to the ``step`` method. 34 | - ``time``: the time of the command, starts at 0.0, then is incremented by the sample time at each call to the ``step`` method. 35 | - ``record_time_nsec``: the epoch time collected when the data in the current time step is recorded. 36 | - ``tsec``: controller time as specified by the FRI in seconds. See FRI documentation for ``getTimestampSec``. 37 | - ``tnsec``: controller time as specified by the FRI in nanoseconds. See FRI documentation for ``getTimestampNanoSec``. 38 | - ``mp1, ..., mp7``: The measured joint position for the robot. 39 | - ``ip1, ..., ip7``: The ipo joint position for the robot. 40 | - ``mt1, ..., mt7``: The measured torque for the robot. 41 | - ``et1, ..., et7``: The external torque for the robot. 42 | - ``dt``: The sample time specified on the KUKA controller. 43 | 44 | See the `LBRJointSineOverlay.py `_:octicon:`link-external` example that demonstrates how to easily collect data from the robot. 45 | 46 | Example Applications 47 | ~~~~~~~~~~~~~~~~~~~~ 48 | .. note:: 49 | Make sure you followed :doc:`Hardware Setup <../../../lbr_fri_ros2_stack/lbr_fri_ros2_stack/doc/hardware_setup>` first. 50 | 51 | .. warning:: 52 | Do always execute in ``T1`` mode first. 53 | 54 | First, ensure the corresponding Java applications for each example are installed (these were supplied with KUKA Sunrise). 55 | Then turn on the robot, connect your laptop via ethernet, and follow these steps on your laptop. 56 | 57 | 1. Change directory: ``cd /path/to/FRI-Client-SDK_Python/examples`` 58 | 2. Run examples 59 | 60 | Hand-guiding 61 | ^^^^^^^^^^^^ 62 | Source code: `hand_guide.py `_:octicon:`link-external` 63 | 64 | This demos allows the user to hand-guide the robot's end-effector through estimating applied forces via the external joint torques. 65 | 66 | #. Remote side configurations: 67 | 68 | #. .. dropdown:: Launch the ``LBRServer`` application on the ``KUKA smartPAD`` 69 | 70 | .. thumbnail:: img/applications_lbr_server.png 71 | 72 | #. Select 73 | 74 | - ``FRI send period``: ``10 ms`` 75 | - ``IP address``: ``your configuration`` 76 | - ``FRI control mode``: ``POSITION_CONTROL`` 77 | - ``FRI client command mode``: ``POSITION`` 78 | 79 | #. Client side: 80 | 81 | .. code-block:: bash 82 | 83 | python3 hand_guide.py --lbr-ver 7 84 | 85 | Now gently move the robot's end-effector to see the robot follow your movements. 86 | 87 | Joint Teleoperation 88 | ^^^^^^^^^^^^^^^^^^^ 89 | Source code: `joint_teleop.py `_:octicon:`link-external` 90 | 91 | This example application lets users control individual joints via the keyboard. 92 | 93 | #. Remote side configurations: 94 | 95 | #. .. dropdown:: Launch the ``LBRServer`` application on the ``KUKA smartPAD`` 96 | 97 | .. thumbnail:: img/applications_lbr_server.png 98 | 99 | #. Select 100 | 101 | - ``FRI send period``: ``10 ms`` 102 | - ``IP address``: ``your configuration`` 103 | - ``FRI control mode``: ``POSITION_CONTROL`` 104 | - ``FRI client command mode``: ``POSITION`` 105 | 106 | #. On the client side, run: 107 | 108 | .. code-block:: bash 109 | 110 | python3 joint_teleop.py 111 | 112 | Press ``1-7`` to enable / disable the corresponding joint. Press ``left`` / ``right`` arrow keys to move the joint. 113 | 114 | Task Teleoperation 115 | ^^^^^^^^^^^^^^^^^^ 116 | Source code: `task_teleop.py `_:octicon:`link-external` 117 | 118 | This example application lets users control the end-effector of the robot via the keyboard. 119 | 120 | #. Remote side configurations: 121 | 122 | #. .. dropdown:: Launch the ``LBRServer`` application on the ``KUKA smartPAD`` 123 | 124 | .. thumbnail:: img/applications_lbr_server.png 125 | 126 | #. Select 127 | 128 | - ``FRI send period``: ``10 ms`` 129 | - ``IP address``: ``your configuration`` 130 | - ``FRI control mode``: ``POSITION_CONTROL`` 131 | - ``FRI client command mode``: ``POSITION`` 132 | 133 | #. On the client side, run: 134 | 135 | .. code-block:: bash 136 | 137 | python3 task_teleop.py --lbr-ver 7 # or 14 if you are using med14 / iiwa14 138 | 139 | Press ``x, y, z, r, p, a`` to enable the a specific axis. Press ``left`` / ``right`` arrow keys to move the axis. 140 | 141 | KUKA Default Demos 142 | ^^^^^^^^^^^^^^^^^^ 143 | These demos mirror the demos that are provided with the C++ FRI client SDK from KUKA, only using the Python bindings instead. 144 | 145 | LBR Joint Sine Overlay 146 | """""""""""""""""""""" 147 | Source code: `LBRJointSineOverlay.py `_:octicon:`link-external` 148 | 149 | #. Remote side configurations: 150 | 151 | #. .. dropdown:: Launch the ``LBRServer`` application on the ``KUKA smartPAD`` 152 | 153 | .. thumbnail:: img/applications_lbr_server.png 154 | 155 | #. Select 156 | 157 | - ``FRI send period``: ``10 ms`` 158 | - ``IP address``: ``your configuration`` 159 | - ``FRI control mode``: ``POSITION_CONTROL`` 160 | - ``FRI client command mode``: ``POSITION`` 161 | 162 | #. Client side: 163 | 164 | .. code-block:: bash 165 | 166 | python3 LBRJointSineOverlay.py # based on examples provided by KUKA 167 | 168 | You should see joint ``A4`` moving in a sine wave. 169 | 170 | LBR Torque Sine Overlay 171 | """"""""""""""""""""""" 172 | Source code: `LBRTorqueSineOverlay.py `_:octicon:`link-external` 173 | 174 | #. Remote side configurations: 175 | 176 | #. .. dropdown:: Launch the ``LBRServer`` application on the ``KUKA smartPAD`` 177 | 178 | .. thumbnail:: img/applications_lbr_server.png 179 | 180 | #. Select 181 | 182 | - ``FRI send period``: ``2 ms`` 183 | - ``IP address``: ``your configuration`` 184 | - ``FRI control mode``: ``JOINT_IMPEDANCE_CONTROL`` 185 | - ``FRI client command mode``: ``TORQUE`` 186 | 187 | #. Client side: 188 | 189 | .. code-block:: bash 190 | 191 | python3 LBRTorqueSineOverlay.py # based on examples provided by KUKA 192 | 193 | You should see joint ``A4`` moving in a sine wave. 194 | 195 | LBR Wrench Sine Overlay 196 | """"""""""""""""""""""" 197 | Source code: `LBRWrenchSineOverlay.py `_:octicon:`link-external` 198 | 199 | #. Remote side configurations: 200 | 201 | #. .. dropdown:: Launch the ``LBRServer`` application on the ``KUKA smartPAD`` 202 | 203 | .. thumbnail:: img/applications_lbr_server.png 204 | 205 | #. Select 206 | 207 | - ``FRI send period``: ``2 ms`` 208 | - ``IP address``: ``your configuration`` 209 | - ``FRI control mode``: ``CARTESIAN_IMPEDANCE_CONTROL`` 210 | - ``FRI client command mode``: ``WRENCH`` 211 | 212 | #. Client side: 213 | 214 | .. code-block:: bash 215 | 216 | python3 LBRWrenchSineOverlay.py # based on examples provided by KUKA 217 | 218 | You should see the end-effector circulating. 219 | -------------------------------------------------------------------------------- /pyfri/tools/state_estimators.py: -------------------------------------------------------------------------------- 1 | import abc 2 | import numpy as np 3 | from pyfri import LBRState 4 | from collections import deque 5 | 6 | 7 | # 8 | # Joint state estimator 9 | # 10 | 11 | 12 | class JointStateEstimator: 13 | """ 14 | 15 | JointStateEstimator 16 | =================== 17 | 18 | The JointStateEstimator class keeps track of the joint position, 19 | velocity, and acceleration using the finite difference method. The 20 | joint velocities and accelerations are estimated using a window of 21 | the previous three joint positions. 22 | 23 | """ 24 | 25 | n_window = 3 26 | 27 | def __init__( 28 | self, 29 | client, 30 | ): 31 | # Set class attributes/variables 32 | self._client = client 33 | self._first_update = True 34 | self._q = deque([], maxlen=self.n_window) 35 | self._dq = deque([], maxlen=self.n_window - 1) 36 | self._ddq = deque([], maxlen=self.n_window - 2) 37 | 38 | # Monkey patch update_window into command method 39 | orig_command = self._client.command 40 | 41 | def command(*args, **kwargs): 42 | self._update_window() 43 | orig_command(*args, **kwargs) 44 | 45 | self._client.command = command 46 | 47 | def q(self, idx): 48 | return np.array(self._q[idx]) 49 | 50 | def dq(self, idx): 51 | return np.array(self._dq[idx]) 52 | 53 | def ddq(self, idx): 54 | return np.array(self._ddq[idx]) 55 | 56 | def _update_window(self): 57 | # Retrieve state 58 | dt = self._client.robotState().getSampleTime() 59 | q = self._client.robotState().getMeasuredJointPosition().flatten().tolist() 60 | 61 | # Update window 62 | self._q.append(q) 63 | if self._first_update: 64 | for _ in range(self.n_window): 65 | self._q.append(q) 66 | self._dq.append([0.0] * LBRState.NUMBER_OF_JOINTS) 67 | self._ddq.append([0.0] * LBRState.NUMBER_OF_JOINTS) 68 | self._first_update = False 69 | 70 | dq = (self.q(-1) - self.q(-2)) / dt 71 | self._dq.append(dq.tolist()) 72 | 73 | ddq = (self.dq(-1) - self.dq(-2)) / dt 74 | self._ddq.append(ddq.tolist()) 75 | 76 | def get_position(self): 77 | return self.q(-1) 78 | 79 | def get_velocity(self): 80 | return self.dq(-1) 81 | 82 | def get_acceleration(self): 83 | return self.ddq(-1) 84 | 85 | 86 | class TaskSpaceStateEstimator: 87 | """ 88 | 89 | TaskSpaceStateEstimator 90 | ======================= 91 | 92 | The TaskSpaceStateEstimator class allows you to estimate a given 93 | end-effector transform (position and orientation), velocity, and 94 | acceleration. 95 | 96 | """ 97 | 98 | def __init__( 99 | self, client, joint_state_estimator, robot_model, ee_link, base_link=None 100 | ): 101 | self._client = client 102 | self._joint_state_estimator = joint_state_estimator 103 | 104 | # Retrieve kinematics models function 105 | if base_link is None: 106 | self._T = robot_model.get_global_link_transform_function( 107 | ee_link, numpy_output=True 108 | ) 109 | self._J = robot_model.get_global_link_geometric_jacobian_function( 110 | ee_link, numpy_output=True 111 | ) 112 | elif isinstance(base_link, str): 113 | self._T = robot_model.get_link_transform_function( 114 | ee_link, base_link, numpy_output=True 115 | ) 116 | self._J = robot_model.get_link_geometric_jacobian_function( 117 | ee_link, base_link, numpy_output=True 118 | ) 119 | else: 120 | raise ValueError(f"{base_link=} was not recognized") 121 | 122 | def get_transform(self): 123 | q = self._joint_state_estimator.get_position() 124 | return self._T(q) 125 | 126 | def get_velocity(self): 127 | q = self._joint_state_estimator.get_position() 128 | dq = self._joint_state_estimator.get_velocity() 129 | J = self._J(q) 130 | return J @ dq 131 | 132 | def get_acceleration(self): 133 | # Retreive joint states 134 | q = self._joint_state_estimator.q(-1) 135 | qp = self._joint_state_estimator.q(-2) 136 | dq = self._joint_state_estimator.dq(-1) 137 | dqp = self._joint_state_estimator.dq(-2) 138 | 139 | # Compute end-effector current and previous velocity 140 | v = self._J(q) @ dq 141 | vp = self._J(qp) @ dqp 142 | 143 | # Compute and return end-effector acceleration 144 | dt = self._client.robotState().getSampleTime() 145 | return (v - vp) / dt 146 | 147 | 148 | class ExternalTorqueEstimator(abc.ABC): 149 | @abc.abstractmethod 150 | def get_external_torque(self): 151 | pass 152 | 153 | 154 | class FRIExternalTorqueEstimator(ExternalTorqueEstimator): 155 | def __init__(self, client): 156 | self._client = client 157 | 158 | def get_external_torque(self): 159 | return self._client.robotState().getExternalTorque().flatten() 160 | 161 | 162 | # 163 | # Estimate wrench at a given tip link. 164 | # 165 | 166 | 167 | class WrenchEstimator(abc.ABC): 168 | """ 169 | 170 | WrenchEstimator 171 | =============== 172 | 173 | Sub-classes of the WrenchEstimator can be used to estimate the 174 | external wrench applied at a given link on the robot, i.e. the tip 175 | link. These methods map the measured external joint torques to the 176 | given task space. An offset is applied to remove bias in the 177 | measurements. How that offset is estimated is implemented by the 178 | respective sub-classes. 179 | 180 | """ 181 | 182 | _rcond = 0.05 # Cutoff for small singular values in pseudo-inverse calculation. 183 | 184 | def __init__( 185 | self, 186 | client, 187 | joint_state_estimator, 188 | external_torque_estimator, 189 | robot_model, 190 | tip_link, 191 | base_link=None, 192 | n_data=50, 193 | ): 194 | # Create class attributes 195 | self._joint_state_estimator = joint_state_estimator 196 | self._external_torque_estimator = external_torque_estimator 197 | 198 | # Setup jacobian function 199 | if base_link is None: 200 | self._jacobian = robot_model.get_global_link_geometric_jacobian_function( 201 | tip_link, 202 | numpy_output=True, 203 | ) 204 | elif isinstance(base_link, str): 205 | self._jacobian = robot_model.get_link_geometric_jacobian_function( 206 | tip_link, 207 | base_link, 208 | numpy_output=True, 209 | ) 210 | else: 211 | raise ValueError(f"{base_link=} is not recognized.") 212 | 213 | # Setup data collector 214 | self._n_data = n_data 215 | self._data = [] 216 | 217 | def _inverse_jacobian(self): 218 | q = self._joint_state_estimator.get_position() 219 | return np.linalg.pinv(self._jacobian(q), rcond=self._rcond) 220 | 221 | def ready(self): 222 | return len(self._data) >= self._n_data 223 | 224 | def update(self): 225 | if len(self._data) < self._n_data: 226 | self._update_data() 227 | 228 | @abc.abstractmethod 229 | def _update_data(self): 230 | pass 231 | 232 | @abc.abstractmethod 233 | def get_wrench(self): 234 | pass 235 | 236 | 237 | class WrenchEstimatorJointOffset(WrenchEstimator): 238 | """ 239 | 240 | WrenchEstimatorJointOffset 241 | ========================== 242 | 243 | The offset is computed in the joint space and applied prior to the 244 | wrench being estimated. 245 | 246 | """ 247 | 248 | def _update_data(self): 249 | tau_ext = self._external_torque_estimator.get_external_torque() 250 | self._data.append(tau_ext.tolist()) 251 | 252 | def get_wrench(self): 253 | offset = np.mean(self._data, axis=0) 254 | tau_ext = self._external_torque_estimator.get_external_torque() - offset 255 | Jinv = self._inverse_jacobian() 256 | return Jinv.T @ tau_ext 257 | 258 | 259 | class WrenchEstimatorTaskOffset(WrenchEstimator): 260 | """ 261 | 262 | WrenchEstimatorTaskOffset 263 | ========================= 264 | 265 | The offset is computed in the task space and applied after the raw 266 | joint torque values are projected to the task space. 267 | 268 | """ 269 | 270 | def _update_data(self): 271 | tau_ext = self._external_torque_estimator.get_external_torque() 272 | f_ext = self._inverse_jacobian().T @ tau_ext 273 | self._data.append(f_ext.flatten().tolist()) 274 | 275 | def get_wrench(self): 276 | offset = np.mean(self._data, axis=0) 277 | tau_ext = self._external_torque_estimator.get_external_torque() 278 | return self._inverse_jacobian().T @ tau_ext - offset 279 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Apache License 2 | Version 2.0, January 2004 3 | http://www.apache.org/licenses/ 4 | 5 | TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION 6 | 7 | 1. Definitions. 8 | 9 | "License" shall mean the terms and conditions for use, reproduction, 10 | and distribution as defined by Sections 1 through 9 of this document. 11 | 12 | "Licensor" shall mean the copyright owner or entity authorized by 13 | the copyright owner that is granting the License. 14 | 15 | "Legal Entity" shall mean the union of the acting entity and all 16 | other entities that control, are controlled by, or are under common 17 | control with that entity. For the purposes of this definition, 18 | "control" means (i) the power, direct or indirect, to cause the 19 | direction or management of such entity, whether by contract or 20 | otherwise, or (ii) ownership of fifty percent (50%) or more of the 21 | outstanding shares, or (iii) beneficial ownership of such entity. 22 | 23 | "You" (or "Your") shall mean an individual or Legal Entity 24 | exercising permissions granted by this License. 25 | 26 | "Source" form shall mean the preferred form for making modifications, 27 | including but not limited to software source code, documentation 28 | source, and configuration files. 29 | 30 | "Object" form shall mean any form resulting from mechanical 31 | transformation or translation of a Source form, including but 32 | not limited to compiled object code, generated documentation, 33 | and conversions to other media types. 34 | 35 | "Work" shall mean the work of authorship, whether in Source or 36 | Object form, made available under the License, as indicated by a 37 | copyright notice that is included in or attached to the work 38 | (an example is provided in the Appendix below). 39 | 40 | "Derivative Works" shall mean any work, whether in Source or Object 41 | form, that is based on (or derived from) the Work and for which the 42 | editorial revisions, annotations, elaborations, or other modifications 43 | represent, as a whole, an original work of authorship. For the purposes 44 | of this License, Derivative Works shall not include works that remain 45 | separable from, or merely link (or bind by name) to the interfaces of, 46 | the Work and Derivative Works thereof. 47 | 48 | "Contribution" shall mean any work of authorship, including 49 | the original version of the Work and any modifications or additions 50 | to that Work or Derivative Works thereof, that is intentionally 51 | submitted to Licensor for inclusion in the Work by the copyright owner 52 | or by an individual or Legal Entity authorized to submit on behalf of 53 | the copyright owner. For the purposes of this definition, "submitted" 54 | means any form of electronic, verbal, or written communication sent 55 | to the Licensor or its representatives, including but not limited to 56 | communication on electronic mailing lists, source code control systems, 57 | and issue tracking systems that are managed by, or on behalf of, the 58 | Licensor for the purpose of discussing and improving the Work, but 59 | excluding communication that is conspicuously marked or otherwise 60 | designated in writing by the copyright owner as "Not a Contribution." 61 | 62 | "Contributor" shall mean Licensor and any individual or Legal Entity 63 | on behalf of whom a Contribution has been received by Licensor and 64 | subsequently incorporated within the Work. 65 | 66 | 2. Grant of Copyright License. Subject to the terms and conditions of 67 | this License, each Contributor hereby grants to You a perpetual, 68 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 69 | copyright license to reproduce, prepare Derivative Works of, 70 | publicly display, publicly perform, sublicense, and distribute the 71 | Work and such Derivative Works in Source or Object form. 72 | 73 | 3. Grant of Patent License. Subject to the terms and conditions of 74 | this License, each Contributor hereby grants to You a perpetual, 75 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 76 | (except as stated in this section) patent license to make, have made, 77 | use, offer to sell, sell, import, and otherwise transfer the Work, 78 | where such license applies only to those patent claims licensable 79 | by such Contributor that are necessarily infringed by their 80 | Contribution(s) alone or by combination of their Contribution(s) 81 | with the Work to which such Contribution(s) was submitted. If You 82 | institute patent litigation against any entity (including a 83 | cross-claim or counterclaim in a lawsuit) alleging that the Work 84 | or a Contribution incorporated within the Work constitutes direct 85 | or contributory patent infringement, then any patent licenses 86 | granted to You under this License for that Work shall terminate 87 | as of the date such litigation is filed. 88 | 89 | 4. Redistribution. You may reproduce and distribute copies of the 90 | Work or Derivative Works thereof in any medium, with or without 91 | modifications, and in Source or Object form, provided that You 92 | meet the following conditions: 93 | 94 | (a) You must give any other recipients of the Work or 95 | Derivative Works a copy of this License; and 96 | 97 | (b) You must cause any modified files to carry prominent notices 98 | stating that You changed the files; and 99 | 100 | (c) You must retain, in the Source form of any Derivative Works 101 | that You distribute, all copyright, patent, trademark, and 102 | attribution notices from the Source form of the Work, 103 | excluding those notices that do not pertain to any part of 104 | the Derivative Works; and 105 | 106 | (d) If the Work includes a "NOTICE" text file as part of its 107 | distribution, then any Derivative Works that You distribute must 108 | include a readable copy of the attribution notices contained 109 | within such NOTICE file, excluding those notices that do not 110 | pertain to any part of the Derivative Works, in at least one 111 | of the following places: within a NOTICE text file distributed 112 | as part of the Derivative Works; within the Source form or 113 | documentation, if provided along with the Derivative Works; or, 114 | within a display generated by the Derivative Works, if and 115 | wherever such third-party notices normally appear. The contents 116 | of the NOTICE file are for informational purposes only and 117 | do not modify the License. You may add Your own attribution 118 | notices within Derivative Works that You distribute, alongside 119 | or as an addendum to the NOTICE text from the Work, provided 120 | that such additional attribution notices cannot be construed 121 | as modifying the License. 122 | 123 | You may add Your own copyright statement to Your modifications and 124 | may provide additional or different license terms and conditions 125 | for use, reproduction, or distribution of Your modifications, or 126 | for any such Derivative Works as a whole, provided Your use, 127 | reproduction, and distribution of the Work otherwise complies with 128 | the conditions stated in this License. 129 | 130 | 5. Submission of Contributions. Unless You explicitly state otherwise, 131 | any Contribution intentionally submitted for inclusion in the Work 132 | by You to the Licensor shall be under the terms and conditions of 133 | this License, without any additional terms or conditions. 134 | Notwithstanding the above, nothing herein shall supersede or modify 135 | the terms of any separate license agreement you may have executed 136 | with Licensor regarding such Contributions. 137 | 138 | 6. Trademarks. This License does not grant permission to use the trade 139 | names, trademarks, service marks, or product names of the Licensor, 140 | except as required for reasonable and customary use in describing the 141 | origin of the Work and reproducing the content of the NOTICE file. 142 | 143 | 7. Disclaimer of Warranty. Unless required by applicable law or 144 | agreed to in writing, Licensor provides the Work (and each 145 | Contributor provides its Contributions) on an "AS IS" BASIS, 146 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or 147 | implied, including, without limitation, any warranties or conditions 148 | of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A 149 | PARTICULAR PURPOSE. You are solely responsible for determining the 150 | appropriateness of using or redistributing the Work and assume any 151 | risks associated with Your exercise of permissions under this License. 152 | 153 | 8. Limitation of Liability. In no event and under no legal theory, 154 | whether in tort (including negligence), contract, or otherwise, 155 | unless required by applicable law (such as deliberate and grossly 156 | negligent acts) or agreed to in writing, shall any Contributor be 157 | liable to You for damages, including any direct, indirect, special, 158 | incidental, or consequential damages of any character arising as a 159 | result of this License or out of the use or inability to use the 160 | Work (including but not limited to damages for loss of goodwill, 161 | work stoppage, computer failure or malfunction, or any and all 162 | other commercial damages or losses), even if such Contributor 163 | has been advised of the possibility of such damages. 164 | 165 | 9. Accepting Warranty or Additional Liability. While redistributing 166 | the Work or Derivative Works thereof, You may choose to offer, 167 | and charge a fee for, acceptance of support, warranty, indemnity, 168 | or other liability obligations and/or rights consistent with this 169 | License. However, in accepting such obligations, You may act only 170 | on Your own behalf and on Your sole responsibility, not on behalf 171 | of any other Contributor, and only if You agree to indemnify, 172 | defend, and hold each Contributor harmless for any liability 173 | incurred by, or claims asserted against, such Contributor by reason 174 | of your accepting any such warranty or additional liability. 175 | 176 | END OF TERMS AND CONDITIONS 177 | 178 | APPENDIX: How to apply the Apache License to your work. 179 | 180 | To apply the Apache License to your work, attach the following 181 | boilerplate notice, with the fields enclosed by brackets "[]" 182 | replaced with your own identifying information. (Don't include 183 | the brackets!) The text should be enclosed in the appropriate 184 | comment syntax for the file format. We also recommend that a 185 | file or class name and description of purpose be included on the 186 | same "printed page" as the copyright notice for easier 187 | identification within third-party archives. 188 | 189 | Copyright [yyyy] [name of copyright owner] 190 | 191 | Licensed under the Apache License, Version 2.0 (the "License"); 192 | you may not use this file except in compliance with the License. 193 | You may obtain a copy of the License at 194 | 195 | http://www.apache.org/licenses/LICENSE-2.0 196 | 197 | Unless required by applicable law or agreed to in writing, software 198 | distributed under the License is distributed on an "AS IS" BASIS, 199 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 200 | See the License for the specific language governing permissions and 201 | limitations under the License. 202 | -------------------------------------------------------------------------------- /examples/robots/med14_description.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | 140 | 141 | 142 | 143 | 144 | 145 | 146 | 147 | 148 | 149 | 150 | 151 | 152 | 153 | 154 | 155 | 156 | 157 | 158 | 159 | 160 | 161 | 162 | 163 | 164 | 165 | 166 | 167 | 168 | 169 | 170 | 171 | 172 | 173 | 174 | 175 | 176 | 177 | 178 | 179 | 180 | 181 | 182 | 183 | 184 | 185 | 186 | 187 | 188 | 189 | 190 | 191 | 192 | 193 | 194 | 195 | 196 | 197 | 198 | 199 | 200 | 201 | 202 | 203 | 204 | 205 | 206 | 207 | 208 | 209 | 210 | 211 | 212 | 213 | 214 | 215 | 216 | 217 | 218 | 219 | 220 | 221 | 222 | 223 | 224 | 225 | 226 | 227 | 228 | 229 | 230 | 231 | 232 | 233 | 234 | 235 | 236 | 237 | 238 | 239 | 240 | 241 | 242 | 243 | 244 | 245 | 246 | 247 | 248 | 249 | 250 | 251 | 252 | 253 | 254 | 255 | 256 | 257 | 258 | 259 | 260 | 261 | 262 | 263 | 264 | 265 | 266 | -------------------------------------------------------------------------------- /examples/robots/med7_description.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | 140 | 141 | 142 | 143 | 144 | 145 | 146 | 147 | 148 | 149 | 150 | 151 | 152 | 153 | 154 | 155 | 156 | 157 | 158 | 159 | 160 | 161 | 162 | 163 | 164 | 165 | 166 | 167 | 168 | 169 | 170 | 171 | 172 | 173 | 174 | 175 | 176 | 177 | 178 | 179 | 180 | 181 | 182 | 183 | 184 | 185 | 186 | 187 | 188 | 189 | 190 | 191 | 192 | 193 | 194 | 195 | 196 | 197 | 198 | 199 | 200 | 201 | 202 | 203 | 204 | 205 | 206 | 207 | 208 | 209 | 210 | 211 | 212 | 213 | 214 | 215 | 216 | 217 | 218 | 219 | 220 | 221 | 222 | 223 | 224 | 225 | 226 | 227 | 228 | 229 | 230 | 231 | 232 | 233 | 234 | 235 | 236 | 237 | 238 | 239 | 240 | 241 | 242 | 243 | 244 | 245 | 246 | 247 | 248 | 249 | 250 | 251 | 252 | 253 | 254 | 255 | 256 | 257 | 258 | 259 | 260 | 261 | 262 | 263 | 264 | 265 | 266 | 267 | 268 | 269 | 270 | 271 | -------------------------------------------------------------------------------- /pyfri/src/wrapper.cpp: -------------------------------------------------------------------------------- 1 | // Standard library 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | // pybind: https://pybind11.readthedocs.io/en/stable/ 10 | #include 11 | #include 12 | 13 | // KUKA FRI-Client-SDK_Cpp (using version hosted at: 14 | // https://github.com/cmower/FRI-Client-SDK_Cpp) 15 | #include "friClientApplication.h" 16 | #include "friClientVersion.h" 17 | #include "friLBRClient.h" 18 | #include "friUdpConnection.h" 19 | 20 | // Function for returning the current time 21 | long long getCurrentTimeInNanoseconds() { 22 | using namespace std::chrono; 23 | 24 | // Use the high-resolution clock for the most accurate time measurement 25 | high_resolution_clock::time_point currentTime = high_resolution_clock::now(); 26 | 27 | // Get the time duration since the epoch in nanoseconds 28 | auto duration = duration_cast(currentTime.time_since_epoch()); 29 | 30 | // Convert the duration to a long long value representing nanoseconds 31 | return duration.count(); 32 | } 33 | 34 | // Make LBRClient a Python abstract class 35 | class PyLBRClient : public KUKA::FRI::LBRClient { 36 | 37 | using KUKA::FRI::LBRClient::LBRClient; 38 | 39 | public: 40 | void onStateChange(KUKA::FRI::ESessionState oldState, 41 | KUKA::FRI::ESessionState newState) override { 42 | PYBIND11_OVERRIDE_PURE(void, LBRClient, onStateChange, oldState, newState); 43 | } 44 | 45 | void monitor() override { PYBIND11_OVERRIDE_PURE(void, LBRClient, monitor); } 46 | 47 | void waitForCommand() override { 48 | PYBIND11_OVERRIDE_PURE(void, LBRClient, waitForCommand); 49 | } 50 | 51 | void command() override { PYBIND11_OVERRIDE_PURE(void, LBRClient, command); } 52 | }; 53 | 54 | // Wrapper for ClientApplication (does not make sense for the user to 55 | // instantiate UdpConnection on the Python side). 56 | class PyClientApplication { 57 | 58 | public: 59 | PyClientApplication(PyLBRClient &client) 60 | : _record_index(0), _collect_data(false), _client(client) { 61 | _app = std::make_unique(_connection, client); 62 | } 63 | 64 | void collect_data(std::string file_name) { 65 | 66 | _collect_data = true; 67 | _time = 0.0; 68 | 69 | // Ensure file name ends with .csv extension 70 | std::string extension = ".csv"; 71 | 72 | if (file_name.length() < extension.length() || 73 | file_name.compare(file_name.length() - extension.length(), 74 | extension.length(), extension) != 0) { 75 | // File name doesn't end with ".csv", so append the extension. 76 | file_name += extension; 77 | } 78 | 79 | _file_name = file_name; 80 | 81 | // Open data file 82 | _data_file.open(_file_name); 83 | 84 | // Write header 85 | _data_file << "index" 86 | << ","; 87 | _data_file << "time" 88 | << ","; 89 | _data_file << "record_time_nsec" 90 | << ","; 91 | _data_file << "tsec" 92 | << ","; 93 | _data_file << "tnsec" 94 | << ","; 95 | 96 | for (unsigned int i = 0; i < KUKA::FRI::LBRState::NUMBER_OF_JOINTS; ++i) 97 | _data_file << "mp" << i + 1 << ","; 98 | 99 | for (unsigned int i = 0; i < KUKA::FRI::LBRState::NUMBER_OF_JOINTS; ++i) 100 | _data_file << "ip" << i + 1 << ","; 101 | 102 | for (unsigned int i = 0; i < KUKA::FRI::LBRState::NUMBER_OF_JOINTS; ++i) 103 | _data_file << "mt" << i + 1 << ","; 104 | 105 | for (unsigned int i = 0; i < KUKA::FRI::LBRState::NUMBER_OF_JOINTS; ++i) 106 | _data_file << "et" << i + 1 << ","; 107 | 108 | _data_file << "dt" 109 | << "\n"; 110 | } 111 | 112 | bool connect(const int port, char *const remoteHost = NULL) { 113 | return _app->connect(port, remoteHost); 114 | } 115 | 116 | void disconnect() { 117 | _app->disconnect(); 118 | if (_collect_data) { 119 | _data_file.close(); 120 | std::cout << "Saved:" << _file_name << "\n"; 121 | } 122 | } 123 | 124 | bool step() { 125 | 126 | // Step FRI 127 | if (!_app->step()) 128 | return false; 129 | 130 | // Optionally record data 131 | KUKA::FRI::ESessionState currentState = 132 | _client.robotState().getSessionState(); 133 | if (_collect_data && 134 | (currentState == KUKA::FRI::ESessionState::COMMANDING_WAIT || 135 | currentState == KUKA::FRI::ESessionState::COMMANDING_ACTIVE)) { 136 | _record_data(); 137 | } 138 | 139 | return true; 140 | } 141 | 142 | private: 143 | unsigned long long _record_index; 144 | long double _time; 145 | bool _collect_data; 146 | std::string _file_name; 147 | std::ofstream _data_file; 148 | PyLBRClient &_client; 149 | KUKA::FRI::UdpConnection _connection; 150 | std::unique_ptr _app; 151 | 152 | void _record_data() { 153 | 154 | // Get state data 155 | double mposition[KUKA::FRI::LBRState::NUMBER_OF_JOINTS]; 156 | memcpy(mposition, _client.robotState().getMeasuredJointPosition(), 157 | KUKA::FRI::LBRState::NUMBER_OF_JOINTS * sizeof(double)); 158 | 159 | double ipoposition[KUKA::FRI::LBRState::NUMBER_OF_JOINTS]; 160 | memcpy(ipoposition, _client.robotState().getIpoJointPosition(), 161 | KUKA::FRI::LBRState::NUMBER_OF_JOINTS * sizeof(double)); 162 | 163 | double mtorque[KUKA::FRI::LBRState::NUMBER_OF_JOINTS]; 164 | memcpy(mtorque, _client.robotState().getMeasuredTorque(), 165 | KUKA::FRI::LBRState::NUMBER_OF_JOINTS * sizeof(double)); 166 | 167 | double ext_torque[KUKA::FRI::LBRState::NUMBER_OF_JOINTS]; 168 | memcpy(ext_torque, _client.robotState().getExternalTorque(), 169 | KUKA::FRI::LBRState::NUMBER_OF_JOINTS * sizeof(double)); 170 | 171 | // Record data 172 | _data_file << _record_index << ","; 173 | _data_file << _time << ","; 174 | _data_file << getCurrentTimeInNanoseconds() << ","; 175 | _data_file << _client.robotState().getTimestampSec() << ","; 176 | _data_file << _client.robotState().getTimestampNanoSec() << ","; 177 | 178 | for (unsigned int i = 0; i < KUKA::FRI::LBRState::NUMBER_OF_JOINTS; ++i) 179 | _data_file << mposition[i] << ","; 180 | 181 | for (unsigned int i = 0; i < KUKA::FRI::LBRState::NUMBER_OF_JOINTS; ++i) 182 | _data_file << ipoposition[i] << ","; 183 | 184 | for (unsigned int i = 0; i < KUKA::FRI::LBRState::NUMBER_OF_JOINTS; ++i) 185 | _data_file << mtorque[i] << ","; 186 | 187 | for (unsigned int i = 0; i < KUKA::FRI::LBRState::NUMBER_OF_JOINTS; ++i) 188 | _data_file << ext_torque[i] << ","; 189 | 190 | double sample_time = _client.robotState().getSampleTime(); 191 | _data_file << sample_time << "\n"; 192 | 193 | // Increment _record_index and _time 194 | _record_index++; 195 | _time += sample_time; 196 | } 197 | }; 198 | 199 | // Python bindings 200 | namespace py = pybind11; 201 | 202 | PYBIND11_MODULE(_pyfri, m) { 203 | m.doc() = "Python bindings for the KUKA FRI Client SDK. THIS IS NOT A KUKA " 204 | "PRODUCT."; 205 | 206 | m.attr("FRI_CLIENT_VERSION_MAJOR") = FRI_CLIENT_VERSION_MAJOR; 207 | m.attr("FRI_CLIENT_VERSION_MINOR") = FRI_CLIENT_VERSION_MINOR; 208 | m.attr("FRI_CLIENT_VERSION") = std::to_string(FRI_CLIENT_VERSION_MAJOR) + 209 | "." + std::to_string(FRI_CLIENT_VERSION_MINOR); 210 | 211 | py::enum_(m, "ESessionState") 212 | .value("IDLE", KUKA::FRI::ESessionState::IDLE) 213 | .value("MONITORING_WAIT", KUKA::FRI::ESessionState::MONITORING_WAIT) 214 | .value("MONITORING_READY", KUKA::FRI::ESessionState::MONITORING_READY) 215 | .value("COMMANDING_WAIT", KUKA::FRI::ESessionState::COMMANDING_WAIT) 216 | .value("COMMANDING_ACTIVE", KUKA::FRI::ESessionState::COMMANDING_ACTIVE) 217 | .export_values(); 218 | 219 | py::enum_(m, "EConnectionQuality") 220 | .value("POOR", KUKA::FRI::EConnectionQuality::POOR) 221 | .value("FAIR", KUKA::FRI::EConnectionQuality::FAIR) 222 | .value("GOOD", KUKA::FRI::EConnectionQuality::GOOD) 223 | .value("EXCELLENT", KUKA::FRI::EConnectionQuality::EXCELLENT) 224 | .export_values(); 225 | 226 | py::enum_(m, "ESafetyState") 227 | .value("NORMAL_OPERATION", KUKA::FRI::ESafetyState::NORMAL_OPERATION) 228 | .value("SAFETY_STOP_LEVEL_0", 229 | KUKA::FRI::ESafetyState::SAFETY_STOP_LEVEL_0) 230 | .value("SAFETY_STOP_LEVEL_1", 231 | KUKA::FRI::ESafetyState::SAFETY_STOP_LEVEL_1) 232 | .value("SAFETY_STOP_LEVEL_2", 233 | KUKA::FRI::ESafetyState::SAFETY_STOP_LEVEL_2) 234 | .export_values(); 235 | 236 | py::enum_(m, "EOperationMode") 237 | .value("TEST_MODE_1", KUKA::FRI::EOperationMode::TEST_MODE_1) 238 | .value("TEST_MODE_2", KUKA::FRI::EOperationMode::TEST_MODE_2) 239 | .value("AUTOMATIC_MODE", KUKA::FRI::EOperationMode::AUTOMATIC_MODE) 240 | .export_values(); 241 | 242 | py::enum_(m, "EDriveState") 243 | .value("OFF", KUKA::FRI::EDriveState::OFF) 244 | .value("TRANSITIONING", KUKA::FRI::EDriveState::TRANSITIONING) 245 | .value("ACTIVE", KUKA::FRI::EDriveState::ACTIVE) 246 | .export_values(); 247 | 248 | py::enum_(m, "EControlMode") 249 | .value("POSITION_CONTROL_MODE", 250 | KUKA::FRI::EControlMode::POSITION_CONTROL_MODE) 251 | .value("CART_IMP_CONTROL_MODE", 252 | KUKA::FRI::EControlMode::CART_IMP_CONTROL_MODE) 253 | .value("JOINT_IMP_CONTROL_MODE", 254 | KUKA::FRI::EControlMode::JOINT_IMP_CONTROL_MODE) 255 | .value("NO_CONTROL", KUKA::FRI::EControlMode::NO_CONTROL) 256 | .export_values(); 257 | 258 | py::enum_(m, "EClientCommandMode") 259 | .value("NO_COMMAND_MODE", KUKA::FRI::EClientCommandMode::NO_COMMAND_MODE) 260 | .value("WRENCH", KUKA::FRI::EClientCommandMode::WRENCH) 261 | .value("TORQUE", KUKA::FRI::EClientCommandMode::TORQUE) 262 | #if FRI_CLIENT_VERSION_MAJOR == 1 263 | .value("POSITION", KUKA::FRI::EClientCommandMode::POSITION) 264 | #elif FRI_CLIENT_VERSION_MAJOR == 2 265 | .value("JOINT_POSITION", KUKA::FRI::EClientCommandMode::JOINT_POSITION) 266 | .value("CARTESIAN_POSE", KUKA::FRI::EClientCommandMode::CARTESIAN_POSE) 267 | #endif 268 | .export_values(); 269 | 270 | py::enum_(m, "EOverlayType") 271 | .value("NO_OVERLAY", KUKA::FRI::EOverlayType::NO_OVERLAY) 272 | .value("JOINT", KUKA::FRI::EOverlayType::JOINT) 273 | .value("CARTESIAN", KUKA::FRI::EOverlayType::CARTESIAN) 274 | .export_values(); 275 | 276 | #if FRI_CLIENT_VERSION_MAJOR == 2 277 | py::enum_(m, "ERedundancyStrategy") 278 | .value("E1", KUKA::FRI::ERedundancyStrategy::E1) 279 | .value("NO_STRATEGY", KUKA::FRI::ERedundancyStrategy::NO_STRATEGY) 280 | .export_values(); 281 | #endif 282 | 283 | py::class_(m, "LBRState") 284 | .def(py::init<>()) 285 | .def_property_readonly_static("NUMBER_OF_JOINTS", 286 | [](py::object) { 287 | int num = 288 | KUKA::FRI::LBRState::NUMBER_OF_JOINTS; 289 | return num; 290 | }) 291 | .def("getSampleTime", &KUKA::FRI::LBRState::getSampleTime) 292 | .def("getSessionState", &KUKA::FRI::LBRState::getSessionState) 293 | .def("getConnectionQuality", &KUKA::FRI::LBRState::getConnectionQuality) 294 | .def("getSafetyState", &KUKA::FRI::LBRState::getSafetyState) 295 | .def("getOperationMode", &KUKA::FRI::LBRState::getOperationMode) 296 | .def("getDriveState", &KUKA::FRI::LBRState::getDriveState) 297 | .def("getClientCommandMode", &KUKA::FRI::LBRState::getClientCommandMode) 298 | .def("getOverlayType", &KUKA::FRI::LBRState::getOverlayType) 299 | .def("getControlMode", &KUKA::FRI::LBRState::getControlMode) 300 | .def("getTimestampSec", &KUKA::FRI::LBRState::getTimestampSec) 301 | .def("getTimestampNanoSec", &KUKA::FRI::LBRState::getTimestampNanoSec) 302 | .def("getMeasuredJointPosition", 303 | [](const KUKA::FRI::LBRState &self) { 304 | // Declare variables 305 | double data[KUKA::FRI::LBRState::NUMBER_OF_JOINTS]; 306 | float dataf[KUKA::FRI::LBRState::NUMBER_OF_JOINTS]; 307 | 308 | // Retrieve state 309 | memcpy(data, self.getMeasuredJointPosition(), 310 | KUKA::FRI::LBRState::NUMBER_OF_JOINTS * sizeof(double)); 311 | 312 | // Parse: double -> float 313 | for (int i = 0; i < KUKA::FRI::LBRState::NUMBER_OF_JOINTS; i++) 314 | dataf[i] = (float)data[i]; 315 | 316 | return py::array_t({KUKA::FRI::LBRState::NUMBER_OF_JOINTS}, 317 | dataf); 318 | }) 319 | .def("getMeasuredTorque", 320 | [](const KUKA::FRI::LBRState &self) { 321 | // Declare variables 322 | double data[KUKA::FRI::LBRState::NUMBER_OF_JOINTS]; 323 | float dataf[KUKA::FRI::LBRState::NUMBER_OF_JOINTS]; 324 | 325 | // Retrieve state 326 | memcpy(data, self.getMeasuredTorque(), 327 | KUKA::FRI::LBRState::NUMBER_OF_JOINTS * sizeof(double)); 328 | 329 | // Parse: double -> float 330 | for (int i = 0; i < KUKA::FRI::LBRState::NUMBER_OF_JOINTS; i++) 331 | dataf[i] = (float)data[i]; 332 | 333 | return py::array_t({KUKA::FRI::LBRState::NUMBER_OF_JOINTS}, 334 | dataf); 335 | }) 336 | .def("getCommandedTorque", 337 | [](const KUKA::FRI::LBRState &self) { 338 | // Declare variables 339 | double data[KUKA::FRI::LBRState::NUMBER_OF_JOINTS]; 340 | float dataf[KUKA::FRI::LBRState::NUMBER_OF_JOINTS]; 341 | 342 | // Retrieve state 343 | memcpy(data, self.getCommandedTorque(), 344 | KUKA::FRI::LBRState::NUMBER_OF_JOINTS * sizeof(double)); 345 | 346 | // Parse: double -> float 347 | for (int i = 0; i < KUKA::FRI::LBRState::NUMBER_OF_JOINTS; i++) 348 | dataf[i] = (float)data[i]; 349 | 350 | return py::array_t({KUKA::FRI::LBRState::NUMBER_OF_JOINTS}, 351 | dataf); 352 | }) 353 | .def("getExternalTorque", 354 | [](const KUKA::FRI::LBRState &self) { 355 | // Declare variables 356 | double data[KUKA::FRI::LBRState::NUMBER_OF_JOINTS]; 357 | float dataf[KUKA::FRI::LBRState::NUMBER_OF_JOINTS]; 358 | 359 | // Retrieve state 360 | memcpy(data, self.getExternalTorque(), 361 | KUKA::FRI::LBRState::NUMBER_OF_JOINTS * sizeof(double)); 362 | 363 | // Parse: double -> float 364 | for (int i = 0; i < KUKA::FRI::LBRState::NUMBER_OF_JOINTS; i++) 365 | dataf[i] = (float)data[i]; 366 | 367 | return py::array_t({KUKA::FRI::LBRState::NUMBER_OF_JOINTS}, 368 | dataf); 369 | }) 370 | .def("getIpoJointPosition", 371 | [](const KUKA::FRI::LBRState &self) { 372 | // Declare variables 373 | double data[KUKA::FRI::LBRState::NUMBER_OF_JOINTS]; 374 | float dataf[KUKA::FRI::LBRState::NUMBER_OF_JOINTS]; 375 | 376 | // Retrieve state 377 | memcpy(data, self.getIpoJointPosition(), 378 | KUKA::FRI::LBRState::NUMBER_OF_JOINTS * sizeof(double)); 379 | 380 | // Parse: double -> float 381 | for (int i = 0; i < KUKA::FRI::LBRState::NUMBER_OF_JOINTS; i++) 382 | dataf[i] = (float)data[i]; 383 | 384 | return py::array_t({KUKA::FRI::LBRState::NUMBER_OF_JOINTS}, 385 | dataf); 386 | }) 387 | .def("getTrackingPerformance", 388 | &KUKA::FRI::LBRState::getTrackingPerformance) 389 | .def("getBooleanIOValue", &KUKA::FRI::LBRState::getBooleanIOValue) 390 | .def("getDigitalIOValue", &KUKA::FRI::LBRState::getDigitalIOValue) 391 | .def("getAnalogIOValue", &KUKA::FRI::LBRState::getAnalogIOValue) 392 | #if FRI_CLIENT_VERSION_MAJOR == 1 393 | .def("getCommandedJointPosition", 394 | [](const KUKA::FRI::LBRState &self) { 395 | // Declare variables 396 | double data[KUKA::FRI::LBRState::NUMBER_OF_JOINTS]; 397 | float dataf[KUKA::FRI::LBRState::NUMBER_OF_JOINTS]; 398 | 399 | // Retrieve state 400 | memcpy(data, self.getCommandedJointPosition(), 401 | KUKA::FRI::LBRState::NUMBER_OF_JOINTS * sizeof(double)); 402 | 403 | // Parse: double -> float 404 | for (int i = 0; i < KUKA::FRI::LBRState::NUMBER_OF_JOINTS; i++) 405 | dataf[i] = (float)data[i]; 406 | 407 | return py::array_t({KUKA::FRI::LBRState::NUMBER_OF_JOINTS}, 408 | dataf); 409 | }) 410 | #elif FRI_CLIENT_VERSION_MAJOR == 2 411 | .def("getMeasuredCartesianPose", 412 | [](const KUKA::FRI::LBRState &self) { 413 | 414 | // Declare variables 415 | double data[KUKA::FRI::LBRState::NUMBER_OF_JOINTS]; 416 | float dataf[KUKA::FRI::LBRState::NUMBER_OF_JOINTS]; 417 | 418 | // Retrieve state 419 | memcpy(data, self.getMeasuredCartesianPose(), 420 | KUKA::FRI::LBRState::NUMBER_OF_JOINTS * sizeof(double)); 421 | 422 | // Parse: double -> float 423 | for (int i = 0; i < KUKA::FRI::LBRState::NUMBER_OF_JOINTS; i++) 424 | dataf[i] = (float)data[i]; 425 | 426 | return py::array_t({KUKA::FRI::LBRState::NUMBER_OF_JOINTS}, 427 | dataf); 428 | }) 429 | .def("getMeasuredCartesianPoseAsMatrix", 430 | [](const KUKA::FRI::LBRState &self) { 431 | // TODO 432 | throw std::runtime_error("getMeasuredCartesianPoseAsMatrix is not yet exposed (use .getMeasuredCartesianPose instead)."); 433 | }) 434 | .def("getIpoCartesianPose", 435 | [](const KUKA::FRI::LBRState &self) { 436 | // TODO 437 | // Currently, FRI Cartesian Overlay is not supported by FRI-Client-SDK_Python. 438 | // IPO Cartesian Pose not available when FRI Cartesian Overlay is not active. 439 | throw std::runtime_error("getIpoCartesianPose is not yet exposed."); 440 | }) 441 | .def("getIpoCartesianPoseAsMatrix", 442 | [](const KUKA::FRI::LBRState &self) { 443 | // TODO 444 | // Currently, FRI Cartesian Overlay is not supported by FRI-Client-SDK_Python. 445 | // IPO Cartesian Pose not available when FRI Cartesian Overlay is not active. 446 | throw std::runtime_error("getIpoCartesianPoseAsMatrix is not yet exposed."); 447 | }) 448 | .def("getMeasuredRedundancyValue", 449 | &KUKA::FRI::LBRState::getMeasuredRedundancyValue) 450 | .def("getIpoRedundancyValue", 451 | [](KUKA::FRI::LBRState &self) { 452 | // TODO 453 | // Currently, FRI Cartesian Overlay is not supported by FRI-Client-SDK_Python. 454 | // IPO redundancy value not available when FRI Cartesian Overlay is not active. 455 | throw std::runtime_error("getIpoRedundancyValue is not yet exposed."); 456 | }) 457 | .def("getRedundancyStrategy", 458 | &KUKA::FRI::LBRState::getRedundancyStrategy) 459 | #endif 460 | ; // NOTE: this completes LBRState 461 | 462 | py::class_(m, "LBRCommand") 463 | .def(py::init<>()) 464 | .def("setJointPosition", 465 | [](KUKA::FRI::LBRCommand &self, py::array_t values) { 466 | if (values.ndim() != 1 || 467 | values.shape(0) != KUKA::FRI::LBRState::NUMBER_OF_JOINTS) { 468 | throw std::runtime_error( 469 | "Input array must have shape (" + 470 | std::to_string(KUKA::FRI::LBRState::NUMBER_OF_JOINTS) + 471 | ",)!"); 472 | } 473 | auto buf = values.request(); 474 | const double *data = static_cast(buf.ptr); 475 | self.setJointPosition(data); 476 | }) 477 | .def("setWrench", 478 | [](KUKA::FRI::LBRCommand &self, py::array_t values) { 479 | if (values.ndim() != 1 || 480 | values.shape(0) != 6 // [F_x, F_y, F_z, tau_A, tau_B, tau_C] 481 | ) { 482 | throw std::runtime_error( 483 | "Input array must have shape (" + 484 | std::to_string(KUKA::FRI::LBRState::NUMBER_OF_JOINTS) + 485 | ",)!"); 486 | } 487 | auto buf = values.request(); 488 | const double *data = static_cast(buf.ptr); 489 | self.setWrench(data); 490 | }) 491 | .def("setTorque", 492 | [](KUKA::FRI::LBRCommand &self, py::array_t values) { 493 | if (values.ndim() != 1 || 494 | values.shape(0) != KUKA::FRI::LBRState::NUMBER_OF_JOINTS) { 495 | throw std::runtime_error( 496 | "Input array must have shape (" + 497 | std::to_string(KUKA::FRI::LBRState::NUMBER_OF_JOINTS) + 498 | ",)!"); 499 | } 500 | auto buf = values.request(); 501 | const double *data = static_cast(buf.ptr); 502 | self.setTorque(data); 503 | }) 504 | .def("setCartesianPose", 505 | [](KUKA::FRI::LBRCommand &self, py::array_t values) { 506 | // TODO 507 | // Currently, FRI Cartesian Overlay is not supported by 508 | // FRI-Client-SDK_Python. 509 | throw std::runtime_error("setCartesianPose is not yet exposed."); 510 | }) 511 | .def("setCartesianPoseAsMatrix", 512 | [](KUKA::FRI::LBRCommand &self, py::array_t values) { 513 | // TODO 514 | // Currently, FRI Cartesian Overlay is not supported by 515 | // FRI-Client-SDK_Python. 516 | throw std::runtime_error( 517 | "setCartesianPoseAsMatrix is not yet exposed."); 518 | }) 519 | .def("setBooleanIOValue", &KUKA::FRI::LBRCommand::setBooleanIOValue) 520 | .def("setDigitalIOValue", &KUKA::FRI::LBRCommand::setDigitalIOValue) 521 | .def("setAnalogIOValue", &KUKA::FRI::LBRCommand::setAnalogIOValue); 522 | 523 | py::class_(m, "LBRClient") 524 | .def(py::init<>()) 525 | .def("onStateChange", &KUKA::FRI::LBRClient::onStateChange) 526 | .def("monitor", &KUKA::FRI::LBRClient::monitor) 527 | .def("waitForCommand", &KUKA::FRI::LBRClient::waitForCommand) 528 | .def("command", &KUKA::FRI::LBRClient::command) 529 | .def("robotState", &KUKA::FRI::LBRClient::robotState) 530 | .def("robotCommand", &KUKA::FRI::LBRClient::robotCommand); 531 | 532 | py::class_(m, "ClientApplication") 533 | .def(py::init()) 534 | .def("connect", &PyClientApplication::connect) 535 | .def("collect_data", &PyClientApplication::collect_data) 536 | .def("disconnect", &PyClientApplication::disconnect) 537 | .def("step", &PyClientApplication::step); 538 | } 539 | --------------------------------------------------------------------------------